hostside/nmra.c

   1 /**/
   2
   3 #include <assert.h>
   4 #include <limits.h>
   5 #include <string.h>
   6
   7 #include "common.h"
   8 #include "nmra.h"
   9
  10 void nmra_encodeforpic(const Nmra *packet, PicInsn *pi) {
  11   const Byte *bp;
  12   int length;
  13   Byte *encp;
  14   unsigned working, newbits;
  15   int working_qty;
  16
  17   length= packet->l;
  18   bp= packet->d;
  19
  20   assert(length > 0);
  21   assert(length <= NMRA_PACKET_MAX);
  22   encp= pi->d;
  23   working= 0xfffc;   /* 16-bit temp register.  Top working_qty bits    */
  24   working_qty= 15;   /*  are some data bits to encode, rest are clear. */
  25   /* we start with the 14-bit preamble and the packet start bit */
  26   for (;;) {
  27     assert(working_qty >= 0);
  28     if (working_qty < 7) {
  29       if (length > 0) {
  30         /* plonk new data bits just to right of old data bits */
  31         length--;
  32         newbits= *bp++;
  33         newbits <<= 1;
  34         newbits |= !length; /* 9 bits, bottom one is `end of packet' */
  35         working |= (newbits << (7-working_qty));
  36         working_qty += 9;
  37       } else if (!working_qty) {
  38         /* all done */
  39         break;
  40       } else {
  41         /* pad with exactly enough 1 bits to make up the encoded byte */
  42         working |= 0xffU << (8-working_qty);
  43         working_qty= 7;
  44       }
  45     }
  46     assert(encp < pi->d + COMMAND_ENCODED_MAX);
  47     *encp++= ((working >> 9) & 0x7f) | 0x80;
  48       /* top 7 bits, right-justified, plus `more command' bit */
  49     working <<= 7;
  50     working_qty -= 7;
  51   }
  52   assert(encp > pi->d);
  53   encp[-1] &= ~0x80; /* clear `more command' bit */
  54   pi->l= encp - pi->d;
  55 }
  56
  57 void nmra_addchecksum(Nmra *packet) {
  58   /* calculates checksum, S9.2 B l.63 */
  59   int left;
  60   Byte *bp;
  61   unsigned running;
  62
  63   assert(packet->l >=0 && packet->l < NMRA_PACKET_MAX);
  64
  65   for (left=packet->l, running=0, bp=packet->d;
  66        left > 0;
  67        bp++, left--)
  68     running ^= *bp;
  69
  70   *bp= running;
  71   packet->l++;
  72 }
  73
  74 static long argnumber(NmraParseEncodeCaller *pec, int argi,
  75                       unsigned long *au, int argc,
  76                       long min, long max) {
  77   long l;
  78   unsigned long abit;
  79
  80   if (argi >= argc) nmra_problem(pec,"missing numeric arg value");
  81
  82   l= nmra_argnumber(pec, argi);
  83
  84   abit= 1ul << argi;
  85   assert(!(*au & abit));
  86   *au |= abit;
  87
  88   if (l<min || l>LONG_MAX || l>max)
  89     nmra_problem(pec,"numeric arg value out of range");
  90   return l;
  91 }
  92
  93 void nmra_parse_encode(Nmra *out, const char *arg, int argl,
  94                        int argc, NmraParseEncodeCaller *pec) {
  95   unsigned long au= 0;
  96
  97   #define Aint(x,i)     , argnumber(pec,i,&au,argc, INT_MIN,INT_MAX)
  98   #define Abitmap(x,i)  , argnumber(pec,i,&au,argc, 0,~0u>>1)
  99   #define Abyte(x,i)    , argnumber(pec,i,&au,argc, 0,0xff)
 100   #define Anone
 101   #define NMRA(n,al,body)                               \
 102   if (argl == sizeof(#n)-1 && !memcmp(arg,#n,argl)) {   \
 103     enco_nmra_##n(out al);                              \
 104   } else
 105 #include "nmra-packets.h"
 106   {
 107     nmra_problem(pec,"unknown instruction");
 108   }
 109   if (au != (1ul << argc)-1)
 110     nmra_problem(pec,"too many args for packet type");
 111 }