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 /*---------- features ----------*/
  56
  57 static void feat_updated_cmd(FeaturesAddr *a, int ix, Nmra *n /*destroyed*/) {
  58   assert(a->rn[ix].pi.l);
  59   retransmit_relaxed_cancel(&a->rn[ix]);
  60   retransmit_relaxed_queue(&a->rn[ix], n);
  61 }
  62
  63 static void feat_updated_funcs(FeaturesAddr *a, int ix,
  64                                void (*enco)(Nmra *n, int addr, unsigned map),
  65                                unsigned thiscmd) {
  66   Nmra n;
  67   if (!(a->all & thiscmd)) return;
  68   enco(&n,a->addr,a->current);
  69   feat_updated_cmd(a,ix,&n);
  70 }
  71
  72 void features_updated(FeaturesAddr *a) {
  73   /* idempotent; call this after updating a->current */
  74
  75   feat_updated_funcs(a,0, enco_nmra_funcs0to4,  0x0001f);
  76   feat_updated_funcs(a,1, enco_nmra_funcs5to8,  0x001e0);
  77   feat_updated_funcs(a,2, enco_nmra_funcs9to12, 0x01e00);
  78
  79   if (a->all & FEATS_SPEEDSTEP_BIT) {
  80     Nmra n;
  81     enco_nmra_speed126(&n, a->addr,
  82                        a->current & FEATS_SPEEDSTEP_BIT
  83                        ? a->speedstep : 0,
  84                        !!(a->all & FEATS_SPEEDSTEP_REVERSE));
  85     feat_updated_cmd(a,3, &n);
  86   }
  87 }
  88
  89 void features_start_xmit(void) {
  90   FeaturesAddr **ap;
  91   Nmra n;
  92   int ix, ai;
  93
  94   for (ai=0,ap=feataddrs; ai<n_feataddrs; ai++,ap++) {
  95     FeaturesAddr *a= *ap;
  96     for (ix=0; ix<FEATURESADDR_TRANSMITS; ix++) {
  97       RetransmitRelaxedNode *rn= &a->rn[ix];
  98       if (!rn->pi.l) {
  99         enco_nmra_idle(&n);
 100         retransmit_relaxed_queue(rn,&n);
 101       }
 102     }
 103     features_updated(a);
 104   }
 105 }