hostside/actual.c

   1 /*
   2  * realtime
   3  * implementation of safety algorithm's decisions
   4  */
   5
   6 #include "realtime.h"
   7 #include "nmra.h"
   8
   9 /*---------- polarity ----------*/
  10
  11 static PicInsn polarityinsn= { { 0x90 }, 0 };
  12
  13 #define OPONBIT_BYTE (polarityinsn.d[bytenum])
  14 #define OPONBIT(body) do{                               \
  15     BoardObject bo;                                     \
  16     int bytenum, bitnum;                                \
  17     Byte bitv;                                          \
  18                                                         \
  19     bo= segi->invert;                                   \
  20     assert(bo);                                         \
  21     bytenum= (bo + 3) / 7;                              \
  22     bitnum= bytenum ? 6 - (bo + 3) % 7 : 3 - bo;        \
  23     bitv= 1u << bitnum;                                 \
  24                                                         \
  25     { body }                                            \
  26   }while(0)
  27
  28 int picinsn_polarity_testbit(const PicInsn *pi, const SegmentInfo *segi) {
  29   OPONBIT({
  30     return !!(OPONBIT_BYTE & bitv);
  31   });
  32 }
  33
  34 void actual_inversions_start(void) {
  35   int i;
  36   polarityinsn.l= (info_maxreverse + 4 + 6) / 7;
  37   for (i=1; i<polarityinsn.l-1; i++)
  38     polarityinsn.d[i] |= 0x80;
  39 }
  40
  41 void actual_inversions_segment(Segment *seg) {
  42   const SegmentInfo *segi= seg->i;
  43
  44   OPONBIT({
  45     Byte *insnbyte= &OPONBIT_BYTE;
  46     if (seg->seg_inverted) *insnbyte |= bitv;
  47     else *insnbyte &= ~bitv;
  48   });
  49 }
  50
  51 void actual_inversions_done(void) {
  52   serial_transmit(&polarityinsn);
  53 }
  54
  55 /*---------- adjucts ----------*/
  56
  57 static void adj_updated_cmd(AdjunctsAddr *a, int ix, Nmra *n /*destroyed*/) {
  58   if (a->rn[ix].pi.l)
  59     retransmit_relaxed_cancel(&a->rn[ix]);
  60   retransmit_relaxed_queue(&a->rn[ix], n);
  61   assert(a->rn[ix].pi.l);
  62 }
  63
  64 static void adj_updated_funcs(AdjunctsAddr *a, int ix,
  65                               void (*enco)(Nmra *n, int addr, unsigned map),
  66                               unsigned thiscmd) {
  67   Nmra n;
  68   if (!(a->all & thiscmd)) return;
  69   enco(&n,a->addr,a->current);
  70   adj_updated_cmd(a,ix,&n);
  71 }
  72
  73 void adjuncts_updated(AdjunctsAddr *a) {
  74   /* idempotent; call this after updating a->current */
  75
  76   adj_updated_funcs(a,0, enco_nmra_funcs0to4,  0x0001f);
  77   adj_updated_funcs(a,1, enco_nmra_funcs5to8,  0x001e0);
  78   adj_updated_funcs(a,2, enco_nmra_funcs9to12, 0x01e00);
  79
  80   if (a->all & ADJS_SPEEDSTEP_BIT) {
  81     Nmra n;
  82     enco_nmra_speed126(&n, a->addr,
  83                        a->current & ADJS_SPEEDSTEP_BIT
  84                        ? a->speedstep : 0,
  85                        !!(a->all & ADJS_SPEEDSTEP_REVERSE));
  86     adj_updated_cmd(a,3, &n);
  87   }
  88 }
  89
  90 void adjuncts_start_xmit(void) {
  91   AdjunctsAddr **ap;
  92   int ai;
  93
  94   for (ai=0,ap=adjaddrs; ai<n_adjaddrs; ai++,ap++) {
  95     AdjunctsAddr *a= *ap;
  96     adjuncts_updated(a);
  97   }
  98 }