simgear/nasal/bitslib.c

   1 #include <string.h>
   2 #include "data.h"
   3
   4 // Note that this currently supports a maximum field width of 32
   5 // bits (i.e. an unsigned int).  Using a 64 bit integer would stretch
   6 // that beyond what is representable in the double result, but
   7 // requires portability work.
   8 #define MSK(n) (1 << (7 - ((n) & 7)))
   9 #define BIT(s,l,n) s[(n)>>3] & MSK(n)
  10 #define CLRB(s,l,n) s[(n)>>3] &= ~MSK(n)
  11 #define SETB(s,l,n) s[(n)>>3] |= MSK(n)
  12
  13 static unsigned int fld(naContext c, unsigned char* s,
  14                         int slen, int bit, int flen)
  15 {
  16     int i;
  17     unsigned int fld = 0;
  18     if(bit + flen > 8*slen) naRuntimeError(c, "bitfield out of bounds");
  19     for(i=0; i<flen; i++) if(BIT(s, slen, bit+flen-i-1)) fld |= (1<<i);
  20     return fld;
  21 }
  22
  23 static void setfld(naContext c, unsigned char* s, int slen,
  24                    int bit, int flen, unsigned int fld)
  25 {
  26     int i;
  27     if(bit + flen > 8*slen) naRuntimeError(c, "bitfield out of bounds");
  28     for(i=0; i<flen; i++)
  29         if(fld & (1<<i)) SETB(s, slen, i+bit);
  30         else CLRB(s, slen, i+bit);
  31 }
  32
  33 static naRef dofld(naContext c, int argc, naRef* args, int sign)
  34 {
  35     naRef s = argc > 0 ? args[0] : naNil();
  36     int bit = argc > 1 ? (int)naNumValue(args[1]).num : -1;
  37     int len = argc > 2 ? (int)naNumValue(args[2]).num : -1;
  38     unsigned int f;
  39     if(!naIsString(s) || !MUTABLE(args[0]) || bit < 0 || len < 0)
  40         naRuntimeError(c, "missing/bad argument to fld/sfld");
  41     f = fld(c, (void*)naStr_data(s), naStr_len(s), bit, len);
  42     if(!sign) return naNum(f);
  43     if(f & (1 << (len-1))) f |= ~((1<<len)-1); // sign extend
  44     return naNum((signed int)f);
  45 }
  46
  47 static naRef f_sfld(naContext c, naRef me, int argc, naRef* args)
  48 {
  49     return dofld(c, argc, args, 1);
  50 }
  51
  52 static naRef f_fld(naContext c, naRef me, int argc, naRef* args)
  53 {
  54     return dofld(c, argc, args, 0);
  55 }
  56
  57 static naRef f_setfld(naContext c, naRef me, int argc, naRef* args)
  58 {
  59     naRef s = argc > 0 ? args[0] : naNil();
  60     int bit = argc > 1 ? (int)naNumValue(args[1]).num : -1;
  61     int len = argc > 2 ? (int)naNumValue(args[2]).num : -1;
  62     naRef val = argc > 3 ? naNumValue(args[3]) : naNil();
  63     if(!argc || !MUTABLE(args[0])|| bit < 0 || len < 0 || IS_NIL(val))
  64         naRuntimeError(c, "missing/bad argument to setfld");
  65     setfld(c, (void*)naStr_data(s), naStr_len(s), bit, len, (unsigned int)val.num);
  66     return naNil();
  67 }
  68
  69 static naRef f_buf(naContext c, naRef me, int argc, naRef* args)
  70 {
  71     naRef len = argc ? naNumValue(args[0]) : naNil();
  72     if(IS_NIL(len)) naRuntimeError(c, "missing/bad argument to buf");
  73     return naStr_buf(naNewString(c), (int)len.num);
  74 }
  75
  76 static naCFuncItem funcs[] = {
  77     { "sfld", f_sfld },
  78     { "fld", f_fld },
  79     { "setfld", f_setfld },
  80     { "buf", f_buf },
  81     { 0 }
  82 };
  83
  84 naRef naInit_bits(naContext c)
  85 {
  86     return naGenLib(c, funcs);
  87 }