hostside/realtime.c

   1 /*
   2  * main program for realtime control
   3  */
   4
   5 #include "realtime.h"
   6
   7 const char *progname= "realtime";
   8
   9 /*---------- global variables ----------*/
  10
  11 CommandInput cmdi;
  12 int picio_send_noise= 1;
  13
  14 static const char *device= "/dev/railway";
  15 static const char *logcopy_fn;
  16
  17 /*---------- general event handling ----------*/
  18
  19 static void comms_error(const char *ch, const char *e1,
  20                         const char *e2 /* may be 0 */) {
  21   if (!e1 && !e2) e1= "end of file";
  22   die("communications error: %s: %s%s%s", ch, e1, e2?": ":"", e2?e2:"");
  23 }
  24
  25 static void *read_exception(oop_source *evts, int fd,
  26                             oop_event evt, void *cl_v) {
  27   const char *ch;
  28   char bufc;
  29   int r;
  30
  31   ch= (fd==cmdi.out.fd ? cmdi.out.desc :
  32        fd==serial_fd ? "serial port" :
  33        0);
  34
  35   r= read(fd, &bufc, 1);
  36   if (r==-1) comms_error(ch, "read error", strerror(errno));
  37   else if (r==0) comms_error(ch, "reports exception, reads EOF", 0);
  38   else comms_error(ch, "reports exception, but readable", 0);
  39
  40   return OOP_CONTINUE;
  41 }
  42
  43 /*---------- logging and output ----------*/
  44
  45 void safety_vpanic(Train *tra, Segment *seg,const char *fmt,va_list al) {
  46   char *msg;
  47   PicInsn piob;
  48
  49   enco_pic_off(&piob);
  50   serial_transmit(&piob);
  51
  52   if (vasprintf(&msg,fmt,al) < 0)
  53     diee("vasprintf failed in safety_vpanic fmt=\"%s\"", fmt);
  54
  55   die("fatal signalling problem: %s at %s: %s",
  56       tra ? tra->pname : "?",
  57       seg ? seg->i->pname : "?",
  58       msg);
  59 }
  60
  61 void safety_panic(Train *tra, Segment *seg, const char *fmt, ...) {
  62   va_list al;
  63   va_start(al,fmt);
  64   safety_vpanic(tra, seg, fmt, al);
  65 }
  66
  67 void ouvprintf(const char *fmt, va_list al) {
  68   ovprintf_ccb(&cmdi.out, simlog_ccb,0, fmt,al);
  69 }
  70 void ouprintf(const char *fmt, ...) {
  71   va_list al;
  72   va_start(al,fmt);
  73   ouvprintf(fmt,al);
  74   va_end(al);
  75 }
  76
  77 void ouvprintf_only(const char *fmt, va_list al) {
  78   ovprintf(&cmdi.out, fmt,al);
  79 }
  80 void ouprintf_only(const char *fmt, ...) {
  81   va_list al;
  82   va_start(al,fmt);
  83   ouvprintf_only(fmt,al);
  84   va_end(al);
  85 }
  86
  87 /*---------- printing nmra data ----------*/
  88
  89 typedef struct {
  90   const Byte *i, *o;
  91   int npending;
  92   unsigned datapending;
  93 } NmraDecCtx;
  94
  95 static void nmradec_init(NmraDecCtx *d, const PicInsn *pi) {
  96   d->i= pi->d;
  97   d->o= pi->d + pi->l;
  98   d->npending= 0;
  99   d->datapending= 0;
 100 }
 101
 102 static void nmradec_getbits(NmraDecCtx *d, int *nbits_io, unsigned *bits_r) {
 103   int use= *nbits_io;
 104   while (d->npending < use) {
 105     if (d->i >= d->o) break;
 106     d->datapending <<= 7;
 107     d->datapending |= *d->i & 0x7f;
 108     d->npending += 7;
 109     if (!(*d->i++ & 0x80)) break;
 110   }
 111   if (d->npending < use) use= d->npending;
 112   d->npending -= use;
 113   *bits_r= (d->datapending >> d->npending) & ~(~0u << use);
 114   *nbits_io= use;
 115 }
 116
 117 static void nmra_decodeforpic(const PicInsn *pi,
 118                               void (*on_idle)(void *u, int),
 119                               void (*on_packet)(void *u, const Nmra*),
 120                               void (*on_error)(void *u, int reasonchar),
 121                               void *u) {
 122   /* reasonchars:
 123    *    #  - bad checksum
 124    *    !  - too little idle
 125    *    $  - truncated
 126    */
 127   NmraDecCtx d;
 128   Nmra n;
 129   int need_idle=7, got_idle=0;
 130   unsigned got, csum;
 131   int nbits;
 132
 133   nmradec_init(&d,pi);
 134   for (;;) {
 135     nbits=1; nmradec_getbits(&d,&nbits,&got);
 136     if (got) { got_idle++; continue; }
 137     if (got_idle) on_idle(u,got_idle);
 138     if (!nbits) return;
 139     if (got_idle < need_idle) on_error(u,'!');
 140     got_idle= 0;
 141
 142     n.l=0; csum=0;
 143     do {
 144       nbits=8; nmradec_getbits(&d,&nbits,&got);
 145       n.d[n.l++]= got;
 146       csum ^= got;
 147       if (nbits<8) on_error(u,'$');
 148
 149       nbits=1; nmradec_getbits(&d,&nbits,&got);
 150       if (nbits<1) on_error(u,'$');
 151     } while (nbits && !got);
 152
 153     if (csum) on_error(u,'#');
 154     else n.l--;
 155
 156     on_packet(u,&n);
 157     need_idle= 14;
 158   }
 159 }
 160
 161 static void opn_idle(void *u, int idle) {
 162   static const char dots[]= "......."; /* at least base/2 */
 163   const int base= 14;
 164   int largers= idle / base;
 165   int units= idle % base;
 166
 167   ouprintf(" %.*s%.*s%.*s",
 168           units/2, dots,
 169           largers, ":::::::" /* enough */,
 170           (units+1)/2, dots);
 171 }
 172 static void opn_error(void *u, int rc) { ouprintf(" %c",rc); }
 173 static void opn_packet(void *u, const Nmra *n) {
 174   int i;
 175   const char *delim= " <";
 176   for (i=0; i<n->l; i++) {
 177     ouprintf("%s%02x",delim,n->d[i]);
 178     delim=" ";
 179   }
 180   ouprintf(">");
 181 }
 182
 183 static void oprint_nmradata(const PicInsn *pi) {
 184   ouprintf("picio out nmradata");
 185   nmra_decodeforpic(pi, opn_idle,opn_packet,opn_error, 0);
 186   ouprintf("\n");
 187 }
 188
 189 /*---------- command channel handling (oop_read, obc) ----------*/
 190
 191 int vbadcmd(ParseState *ps, const char *fmt, va_list al) {
 192   ouprintf("ack %s BadCmd : ", current_cmd?current_cmd->name:"?");
 193   ouvprintf(fmt,al);
 194   ouprintf("\n");
 195   return EC_BadCmd;
 196 }
 197
 198 void oupicio(const char *dirn, const PicInsnInfo *pii, int obj, int v,
 199              void (*qprintf)(const char *fmt, ...)) {
 200   if (!pii->argsbits)
 201     qprintf("picio %s %s\n", dirn, pii->name);
 202   else if (!pii->vbits)
 203     qprintf("picio %s %s %#x\n", dirn, pii->name, obj);
 204   else
 205     qprintf("picio %s %s %#x %d\n", dirn, pii->name, obj, v);
 206 }
 207
 208 static void obc_error(OutBufferChain *ch, const char *e1, const char *e2) {
 209   if (!e1) { assert(!e2); die_hook(); exit(0); }
 210   fprintf(stderr,"command communication error: %s%s%s\n",
 211           e1, e2?": ":"", e2?e2:"");
 212   die_hook();
 213   exit(-1);
 214 }
 215
 216 static void qouhex(const char *word, const Byte *command, int length,
 217                    void (*qprintf)(const char *fmt, ...)) {
 218   qprintf("%s", word);
 219   while (length--) qprintf(" %02x", *command++);
 220   qprintf("\n");
 221 }
 222
 223 void ouhex(const char *word, const Byte *command, int length) {
 224   qouhex(word,command,length, ouprintf);
 225 }
 226 void ouhex_nosim(const char *word, const Byte *command, int length) {
 227   qouhex(word,command,length, ouprintf_only);
 228 }
 229
 230 void die_vprintf_hook(const char *fmt, va_list al) {
 231   static int recursing;
 232   if (!recursing++)
 233     ouvprintf(fmt, al);
 234   recursing--;
 235 }
 236
 237 void die_hook(void) {
 238   PicInsn off;
 239   int e;
 240
 241   enco_pic_off(&off);
 242
 243   write(serial_fd,off.d,off.l);
 244   e= events ? obc_tryflush(&cmdi.out) : 0;
 245   if (e) fprintf(stderr,"(unwritten command output: %s)\n",strerror(e));
 246 }
 247
 248 /*---------- serial input (via oop) ----------*/
 249
 250 PicInsn serial_buf;
 251
 252 static void *serial_readable(oop_source *evts, int fd,
 253                              oop_event evt, void *u0) {
 254   int r, buf_used;
 255
 256   r= read(serial_fd, serial_buf.d + serial_buf.l,
 257           sizeof(serial_buf.d) - serial_buf.l);
 258
 259   if (r==0) die("serial port - eof");
 260   if (r==-1) {
 261     if (errno == EWOULDBLOCK || errno == EINTR)
 262       return OOP_CONTINUE;
 263     diee("serial port - read error");
 264   }
 265   assert(r>0);
 266
 267   buf_used= serial_buf.l + r;
 268   serial_indata_process(buf_used);
 269   return OOP_CONTINUE;
 270 }
 271
 272 void serial_indata_process(int buf_used) {
 273   for (;;) {
 274     serial_buf.l= buf_used;
 275     serial_moredata(&serial_buf);
 276     if (!serial_buf.l) break;
 277     buf_used -= serial_buf.l;
 278     memmove(serial_buf.d, serial_buf.d + serial_buf.l, buf_used);
 279     if (!buf_used) break;
 280   }
 281   serial_buf.l= buf_used;
 282 }
 283
 284 /*---------- serial port output (not via liboop) ----------*/
 285
 286 void serial_transmit(const PicInsn *pi) {
 287   const PicInsnInfo *pii;
 288   int obj, v, suppress=0;
 289
 290   if ((pi->d[0] & 0xf8) == 0x90) {
 291     SEG_IV;
 292     const char *delim;
 293
 294     ouprintf("picio out polarity <");
 295     delim="";
 296     FOR_SEG {
 297       if (!segi->invertible) continue;
 298       if (!picinsn_polarity_testbit(pi,segi)) continue;
 299       ouprintf("%s%s", delim, segi->pname);
 300       delim= ",";
 301     }
 302     ouprintf(">\n");
 303   } else if (pi->d[0] == 0xff) {
 304     if (picio_send_noise < 3)
 305       suppress= 1;
 306     else
 307       oprint_nmradata(pi);
 308   } else {
 309     picinsn_decode(pi, pic_command_infos, &pii, &obj, &v);
 310     if (!pii)
 311       ouprintf("picio out unknown\n");
 312     else if (pii->noiselevel > picio_send_noise)
 313       suppress= 1;
 314     else
 315       oupicio("out",pii,obj,v,ouprintf);
 316   }
 317
 318   if (!suppress && picio_send_noise >= 2)
 319     ouhex("picioh out", pi->d, pi->l);
 320
 321   /* note that the serial port is still in nonblocking mode.  if
 322    * we ever buffer up far enough that the kernel wants to make us
 323    * block, we should die! */
 324   serial_transmit_now(pi->d, pi->l);
 325 }
 326
 327 /*---------- debugging ----------*/
 328
 329 unsigned long eventcounter;
 330
 331 void debug_count_event(const char *what) {
 332   DPRINTF(misc,event, "#0x%lx %s\n",eventcounter,what);
 333   eventcounter++;
 334 }
 335
 336 #define DEFDFLAGS_safety ~(DBIT_safety_predictplan|DBIT_safety_predictseg)
 337 #define DEFDFLAGS_movpos ~(DBIT_movpos_eval|DBIT_movpos_changeneeded)
 338 #define DEFDFLAGS_speed ~(DBIT_speed_query)
 339 #define DEFDFLAGS_retransmit ~(DBIT_retransmit_message)
 340
 341 #define DEBUG_FLAGS_H_DEFINE
 342 #include "realtime+dflags.h"
 343
 344 static int debug_simulate_exactly;
 345
 346 static void debug_user_set(const DebugSelectorAreaInfo *dsai,
 347                            int op, unsigned long bits) {
 348   switch (op) {
 349   case '+':  *dsai->dflags |=  bits;  break;
 350   case '-':  *dsai->dflags &= ~bits;  break;
 351   default: abort();
 352   }
 353   *dsai->userset |= bits;
 354 }
 355
 356 static void debug_arg_spec(const char *arg) {
 357   /* syntaxes:
 358    *  -D+
 359    *  -D-
 360    *  -D+<area>
 361    *  -D-<area>
 362    *  -D<area><kinds>
 363    * eg:
 364    *  -D<area>+<kind>[<kinds>...]
 365    *  -D<area>-<kind>[<kinds>...]
 366    * where <kind> can be `*'
 367    */
 368   int wlen, l, alen, op=0;
 369   const char *delim, *area;
 370   const DebugSelectorAreaInfo *dsai=0;
 371   const DebugSelectorKindInfo *dski;
 372   unsigned long bits;
 373
 374   if (!strcmp("=",arg)) { debug_simulate_exactly= 1; return; }
 375
 376   for (;;) {
 377     /* possibilities, arg points to |   dsai   op
 378      *  -D|                              0      undef
 379      *  -D|?                             0      undef
 380      *  -D|?<area>                       0      undef
 381      *  -D|<area>?...                    0      undef
 382      *  -D<area>...?|<kind>            <area>   char `?'
 383      *  -D<area>...?|<kind>?...        <area>   1st `?'
 384      */
 385
 386     wlen= strcspn(arg,"+-");
 387     delim= &arg[wlen];
 388
 389     if (!dsai) { /* -D|... */
 390       if (!*delim) badusage("-D without any + - or =");
 391       if (wlen) { /* -D|<area>?... */
 392         area= arg;
 393         alen= wlen;
 394       } else { /* -D|?[<area>] */
 395         area= arg+1;
 396         alen= strlen(area);
 397         if (!alen) { /* -D|? */
 398           for (dsai=dsais; dsai->name; dsai++)
 399             debug_user_set(dsai,*delim,~0UL);
 400           return;
 401         }
 402       }
 403       /* -D|<area>?... or -D?<area> */
 404       for (dsai=dsais; dsai->name; dsai++) {
 405         l= strlen(dsai->name);
 406         if (l==alen && !memcmp(dsai->name,area,l)) goto area_found;
 407       }
 408       badusage("unknown debug message area");
 409     area_found:
 410       if (!wlen) { /* -D|?<area> */
 411         debug_user_set(dsai,*delim,~0UL);
 412         return;
 413       }
 414       /* -D|<area>?<kind>... */
 415     } else { /* -D<area>...?|<kind>[?...] */
 416       if (wlen==1 && arg[0]=='*') {
 417         bits= ~0UL;
 418         goto kind_found;
 419       }
 420       for (dski=dsai->kinds; dski->name; dski++) {
 421         l= strlen(dski->name);
 422         if (l==wlen && !memcmp(dski->name,arg,l)) {
 423           bits= dski->bit;
 424           goto kind_found;
 425         }
 426       }
 427       badusage("unknown debug message kind");
 428     kind_found:
 429       debug_user_set(dsai,op,bits);
 430       if (!*delim) /* -D<area>...?|<kind> */
 431         return;
 432        /* -D<area>...?|<kind>?<kind>... */
 433     }
 434     /* -D...?|<something>?<kind>... */
 435     op= *delim++;
 436     arg= delim;
 437     /* -D...?<something>?|<kind>... */
 438   }
 439 }
 440
 441 static void debug_setup(void) {
 442   const DebugSelectorAreaInfo *dsai;
 443   unsigned long def;
 444
 445   for (dsai=dsais; dsai->name; dsai++) {
 446     def= (simulate && !debug_simulate_exactly) ? ~0UL : dsai->defdflags;
 447     *dsai->dflags |= def & ~*dsai->userset;
 448   }
 449 }
 450
 451 /*---------- initialisation ----------*/
 452
 453 int main(int argc, const char **argv) {
 454   oop_source_sys *sys_events;
 455   const char *arg;
 456   int r, c;
 457
 458   persist_map_veryearly();
 459
 460   if (argv[0] && argv[1] && !strcmp(argv[1],PERSIST_CONVERT_OPTION))
 461     /* do this before we call malloc so that MAP_FIXED is sure to work */
 462     persist_entrails_run_converter();
 463
 464   while ((arg=*++argv) && *arg=='-' && arg[1]) {
 465     arg++;
 466     while (arg && *arg) {
 467       switch (*arg++) {
 468       case 's': device= arg;                      arg=0; break;
 469       case 'p': persist_fn= arg;                  arg=0; break;
 470       case 'v': picio_send_noise= atoi(arg);      arg=0; break;
 471       case 'm': sta_state= Sta_Manual;                   break;
 472       case 'V': simlog_full=1;                           break;
 473       case 'L': logcopy_fn= arg;                  arg=0; break;
 474       case 'S': simulate= arg;                    arg=0; break;
 475       case 'D': debug_arg_spec(arg);              arg=0; break;
 476       case 'R':
 477         rtfeats_use= 0;
 478         while ((c= *arg++)) {
 479           switch (c) {
 480           case 'd': rtfeats_use |= RTFEAT_DEFAULTS;     break;
 481           case 'p': rtfeats_use |= RTFEAT_CPU;          break;
 482           case 'P': rtfeats_use |= RTFEAT_ALL(CPU);     break;
 483           case 'm': rtfeats_use |= RTFEAT_MEM;          break;
 484           case 'M': rtfeats_use |= RTFEAT_ALL(MEM);     break;
 485           default: badusage("unknown -R suboption");
 486           }
 487         }
 488         arg= 0;
 489         break;
 490       default: badusage("unknown option");
 491       }
 492     }
 493   }
 494
 495   cmdi.out.desc= (char*)"command";
 496   cmdi.out.fd= 1;
 497   cmdi.out.error= obc_error;
 498   cmdi.doline= command_doline;
 499   cmdi.out.empty= cmdi_output_bufferempty;
 500
 501   debug_setup();
 502
 503   if (!simulate) {
 504     sys_events= oop_sys_new();  if (!sys_events) diee("oop_sys_new");
 505     events= oop_sys_source(sys_events);  massert(events);
 506
 507     simlog_open(logcopy_fn);
 508     cmdin_new(&cmdi, 0);
 509
 510     serial_open(device);
 511     r= oop_fd_nonblock(serial_fd, 1);  if (r) diee("nonblock(serial_fd,1)");
 512
 513     events->on_fd(events, serial_fd, OOP_READ, serial_readable, 0);
 514     events->on_fd(events, serial_fd, OOP_EXCEPTION, read_exception, 0);
 515
 516     if (rtfeats_use & RTFEAT_DEFAULTS)
 517       rtfeats_use |= RTFEAT_CPU | RTFEAT_MEM;
 518   } else {
 519     sim_initialise(logcopy_fn);
 520     sys_events= 0;
 521   }
 522
 523   persist_entrails_interpret();
 524   records_parse(argv);
 525   realtime_priority();
 526   speedmanager_init();
 527   sta_startup();
 528
 529   if (!simulate) oop_sys_run(sys_events);
 530   else sim_run();
 531
 532   abort();
 533 }
 534
 535 DEFINE_ERRORCODELIST_DATA