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