user/sendip/ntp.c

   1 /* ntp.c - Network Timxe Protocol module for sendip
   2  * Author: Mike Ricketts <mike@earth.li>
   3  * ChangeLog since 2.1 release:
   4  */
   5
   6 #include <stdlib.h>
   7 #include <sys/types.h>
   8 #include <math.h>
   9 #include <string.h>
  10 #include <netinet/in.h>
  11 #include <arpa/inet.h>
  12 #include <sys/socket.h>
  13 #include "sendip_module.h"
  14 #include "ntp.h"
  15
  16 /* Character that identifies our options
  17  */
  18 const char opt_char='n';
  19
  20 u_int32_t make_fixed_point(double n, bool issigned, int totbits, int intbits) {
  21         u_int32_t intpart;
  22         u_int32_t fracpart;
  23         u_int32_t result;
  24         bool signbit;
  25         double intpartd, fracpartd;
  26         int fracbits;
  27
  28         if(issigned) totbits--;
  29
  30         fracbits=totbits-intbits;
  31
  32         if(issigned && n<0) {
  33                 signbit=TRUE;
  34         } else {
  35                 signbit=FALSE;
  36         } /* else, signbit is unused */
  37
  38         n=fabs(n);
  39
  40 /*
  41         fracpartd=floor(ldexp(modf(n,&intpartd),ldexp(2.0,fracbits)));
  42 */
  43         fracpartd=floor(ldexp(modf(n,&intpartd),32));
  44         intpart=(u_int32_t)intpartd;
  45         fracpart=(u_int32_t)fracpartd;
  46
  47
  48         if(issigned&&signbit) {
  49                 result=1<<totbits;
  50         } else {
  51                 result=0;
  52         }
  53
  54         if(intbits!=0) {
  55                 intpart&=(1<<intbits)-1;
  56                 intpart<<=(totbits-intbits);
  57                 result|=intpart;
  58         }
  59         if(intbits!=totbits) {
  60                 if(fracbits!=32) {
  61                         fracpart&=((1<<fracbits)-1)<<intbits;
  62                         fracpart>>=intbits;
  63                 }
  64                 result|=fracpart;
  65         }
  66         return htonl(result);
  67 }
  68
  69 bool make_ts(ntp_ts *dest, char *src) {
  70         char *intpart, *fracpart;
  71         intpart=src;
  72         fracpart=strchr(intpart,'.');
  73
  74         if(*intpart) {
  75                 dest->intpart=(u_int32_t)strtoul(intpart,&fracpart,0);
  76         } else {
  77                 dest->intpart=0;
  78         }
  79         fracpart++;  // skip the .
  80         if(fracpart && *fracpart) {
  81                 double d;
  82                 d = strtod(fracpart-1,(char **)NULL);
  83                 dest->fracpart=make_fixed_point(d,FALSE,32,0);
  84         }
  85         return TRUE;
  86 }
  87
  88 sendip_data *initialize(void) {
  89         sendip_data *ret = malloc(sizeof(sendip_data));
  90         ntp_header *ntp = malloc(sizeof(ntp_header));
  91         memset(ntp,0,sizeof(ntp_header));
  92         ret->alloc_len = sizeof(ntp_header);
  93         ret->data = (void *)ntp;
  94         ret->modified=0;
  95         return ret;
  96 }
  97
  98 bool do_opt(char *opt, char *arg, sendip_data *pack) {
  99         ntp_header *ntp = (ntp_header *)pack->data;
 100         switch(opt[1]) {
 101         case 'l':  /* Leap Indicator (2 bits) */
 102                 ntp->leap=(u_int8_t)strtoul(arg,(char **)NULL,0)&3;
 103                 pack->modified|=NTP_MOD_LEAP;
 104                 break;
 105         case 's':  /* Status (6 bits, values 0-4 defined */
 106                 ntp->status=(u_int8_t)strtoul(arg,(char **)NULL,0)&0x3F;
 107                 pack->modified|=NTP_MOD_STATUS;
 108                 break;
 109         case 't':  /* Type (8 bits, values 0-4 defined */
 110                 ntp->type=(u_int8_t)strtoul(arg,(char **)NULL,0)&0xFF;
 111                 pack->modified|=NTP_MOD_TYPE;
 112                 break;
 113         case 'p':  /* precision (16 bits, range +32 to -32) */
 114                 ntp->precision=htons((int8_t)strtol(arg,(char **)NULL,0));
 115                 pack->modified|=NTP_MOD_PRECISION;
 116                 break;
 117         case 'e':  /* esitmated error (32 bits, fixed point between bits 15/16) */
 118                 ntp->error=make_fixed_point(strtod(arg,(char **)NULL),FALSE,32,16);
 119                 pack->modified|=NTP_MOD_ERROR;
 120                 break;
 121         case 'd':  /* estimated drift rate (32 bits, signed fixed point left of
 122                                           high order bit) */
 123                 ntp->drift=make_fixed_point(strtod(arg,(char **)NULL),TRUE,32,0);
 124                 pack->modified|=NTP_MOD_DRIFT;
 125                 break;
 126         case 'r':  /* reference clock id (32 bits or a 4 byte string).  Can be:
 127                                           If type==1: "WWVB", "GOES", "WWV\0"
 128                                           If type==2: IP address
 129                                           Else: must be zero
 130                                   */
 131                 if('0'<=*arg&&*arg<='9') {
 132                         /* either a number or an IP */
 133                         if((ntp->reference.ipaddr=inet_addr(arg))==-1) {
 134                                 /* Not a valid IP, or really want 255.255.255.255 */
 135                                 if(strcmp(arg,"255.255.255.255")) {
 136                                         ntp->reference.ipaddr=htonl(strtol(arg,(char **)NULL,0));
 137                                 }
 138                         }
 139                 } else {
 140                         /* Hopefully a 4 byte or less string */
 141                         ntp->reference.ipaddr = 0;
 142                         if(strlen(arg)<=4) {
 143                                 memcpy(ntp->reference.id,arg,strlen(arg));
 144                         } else {
 145                                 usage_error("NTP reference clock ID must be IP addr, 32 bit integer, or 4 byte string\n");
 146                                 return FALSE;
 147                         }
 148                 }
 149                 pack->modified|=NTP_MOD_REF;
 150                 break;
 151         case 'f':  /* reference timestamp (64 bits) */
 152                 if(make_ts(&(ntp->reference_ts),arg)) {
 153                         pack->modified|=NTP_MOD_REFERENCE;
 154                 } else {
 155                         usage_error("Couldn't parse NTP reference timestamp\n");
 156                         return FALSE;
 157                 }
 158                 break;
 159         case 'o':  /* originate timestamp (64 bits) */
 160                 if(make_ts(&(ntp->originate_ts),arg)) {
 161                         pack->modified|=NTP_MOD_ORIGINATE;
 162                 } else {
 163                         usage_error("Couldn't parse NTP originate timestamp\n");
 164                         return FALSE;
 165                 }
 166                 break;
 167         case 'a':  /* receive timestamp (64 bits) */
 168                 if(make_ts(&(ntp->receive_ts),arg)) {
 169                         pack->modified|=NTP_MOD_RECEIVE;
 170                 } else {
 171                         usage_error("Couldn't parse NTP receive timestamp\n");
 172                         return FALSE;
 173                 }
 174                 break;
 175         case 'x':  /* transmit timestamp (64 bits) */
 176                 if(make_ts(&(ntp->transmit_ts),arg)) {
 177                         pack->modified|=NTP_MOD_TRANSMIT;
 178                 } else {
 179                         usage_error("Couldn't parse NTP transmit timestamp\n");
 180                         return FALSE;
 181                 }
 182                 break;
 183         }
 184         return TRUE;
 185
 186 }
 187
 188 bool finalize(char *hdrs, sendip_data *headers[], sendip_data *data,
 189                                   sendip_data *pack) {
 190         if(hdrs[strlen(hdrs)-1] != 'u') {
 191                 usage_error("Warning: NTP should be contained in a UDP packet\n");
 192         }
 193         return TRUE;
 194 }
 195
 196 int num_opts() {
 197         return sizeof(ntp_opts)/sizeof(sendip_option);
 198 }
 199 sendip_option *get_opts() {
 200         return ntp_opts;
 201 }
 202 char get_optchar() {
 203         return opt_char;
 204 }