git.sur5r.net Git - ngadmin/blob - lib/src/protocol.c

   1
   2 #include "protocol.h"
   3
   4
   5
   6
   7 // ----------------------------
   8 int trim (char *txt, int start) {
   9
  10  char *p, c;
  11
  12
  13  if ( txt==NULL ) {
  14   return 0;
  15  }
  16
  17  //for (p=txt; *p!=0; p++);
  18  p=txt+start;
  19  for (p--; p>=txt && ( (c=*p)==' ' || c=='\n' ); *p--=0);
  20
  21
  22  return p-txt+1;
  23
  24 }
  25
  26
  27
  28 // -------------------
  29 int min (int a, int b) {
  30  return a<b ? a : b ;
  31 }
  32
  33
  34
  35 // -----------------------------------------------------------------------------------------------------------------------------------------------
  36 void initNgHeader (struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum) {
  37
  38
  39  memset(nh, 0, sizeof(struct ng_header));
  40  nh->version=1;
  41  nh->code=code;
  42
  43  memcpy(nh->client_mac, client_mac, ETH_ALEN);
  44
  45  if ( switch_mac!=NULL ) {
  46   memcpy(nh->switch_mac, switch_mac, ETH_ALEN);
  47  }
  48
  49  nh->seqnum=htonl(seqnum);
  50  strcpy(nh->proto_id, "NSDP");
  51
  52
  53 }
  54
  55
  56
  57 // ---------------------------------------------------------------------------------------------------------------------------------------------------------
  58 bool validateNgHeader (const struct ng_header *nh, char code, const struct ether_addr *client_mac, const struct ether_addr *switch_mac, unsigned int seqnum) {
  59
  60
  61  if ( nh->version!=1 ) {
  62   return false;
  63  }
  64
  65  if ( code>0 && nh->code!=code ) {
  66   return false;
  67  }
  68
  69  if ( nh->unk1!=0 ) {
  70   return false;
  71  }
  72
  73  if ( *(unsigned short*)nh->unk2!=0 ) {
  74   return false;
  75  }
  76
  77  if ( client_mac!=NULL && memcmp(nh->client_mac, client_mac, ETH_ALEN)!=0 ) {
  78   return false;
  79  }
  80
  81  if ( switch_mac!=NULL && memcmp(nh->switch_mac, switch_mac, ETH_ALEN)!=0 ) {
  82   return false;
  83  }
  84
  85  if ( seqnum>0 && ntohl(nh->seqnum)!=seqnum ) {
  86   return false;
  87  }
  88
  89  if ( memcmp(nh->proto_id, "NSDP", 4)!=0 ) {
  90   return false;
  91  }
  92
  93  if ( *(unsigned int*)nh->unk3!=0 ) {
  94   return false;
  95  }
  96
  97
  98  return true;
  99
 100 }
 101
 102
 103
 104 // -------------------------------------
 105 void initNgPacket (struct ng_packet *np) {
 106
 107  np->ah=(struct attr_header*)np->nh->data;
 108
 109 }
 110
 111
 112
 113 // --------------------------------------------------------------------------------------------
 114 void addPacketAttr (struct ng_packet *np, unsigned short attr, unsigned short size, void* data) {
 115
 116  struct attr_header *ah=np->ah;
 117
 118
 119  if ( (int)(getPacketTotalSize(np)+sizeof(struct attr_header)+size)>(np->maxlen) ) {
 120   return;
 121  }
 122
 123  ah->attr=htons(attr);
 124  ah->size=htons(size);
 125
 126  if ( size>0 && data!=NULL ) {
 127   memcpy(ah->data, data, size);
 128  }
 129
 130  np->ah=(struct attr_header*)(ah->data+size);
 131
 132 }
 133
 134
 135
 136 // ----------------------------------------------------------------
 137 void addPacketEmptyAttr (struct ng_packet *np, unsigned short attr) {
 138  addPacketAttr(np, attr, 0, NULL);
 139 }
 140
 141
 142
 143 // -------------------------------------------------------------------------
 144 void addPacketByteAttr (struct ng_packet *np, unsigned short attr, char val) {
 145  addPacketAttr(np, attr, 1, &val);
 146 }
 147
 148
 149
 150 // ---------------------------------------------------------------------------
 151 void addPacketShortAttr (struct ng_packet *np, unsigned short attr, short val) {
 152
 153  short s=htons(val);
 154
 155
 156  addPacketAttr(np, attr, 2, &s);
 157
 158 }
 159
 160
 161
 162 // ------------------------------------------------
 163 int getPacketTotalSize (const struct ng_packet *np) {
 164  return ((char*)np->ah)-np->buffer;
 165 }
 166
 167
 168
 169 // --------------------------------------------
 170 struct attr* newEmptyAttr (unsigned short attr) {
 171  return newAttr(attr, 0, NULL);
 172 }
 173
 174
 175
 176 // ------------------------------------------------------------------------
 177 struct attr* newAttr (unsigned short attr, unsigned short size, void *data) {
 178
 179  struct attr *at;
 180
 181
 182  at=malloc(sizeof(struct attr));
 183  at->attr=attr;
 184  at->size=size;
 185  at->data=data;
 186
 187
 188  return at;
 189
 190 }
 191
 192
 193
 194 // ----------------------------------------------------------------
 195 struct attr* newByteAttr (unsigned short attr, unsigned char value) {
 196
 197  char *v=malloc(sizeof(char));
 198
 199  *v=value;
 200
 201  return newAttr(attr, sizeof(char), v);
 202
 203 }
 204
 205
 206
 207 // ---------------------------------------------------------
 208 struct attr* newShortAttr (unsigned short attr, short value) {
 209
 210  short *v=malloc(sizeof(short));
 211
 212  *v=htons(value);
 213
 214  return newAttr(attr, sizeof(short), v);
 215
 216 }
 217
 218
 219
 220 // -----------------------------------------------------
 221 struct attr* newIntAttr (unsigned short attr, int value) {
 222
 223  int *v=malloc(sizeof(int));
 224
 225  *v=htonl(value);
 226
 227  return newAttr(attr, sizeof(int), v);
 228
 229 }
 230
 231
 232
 233 // -----------------------------------------------------------------
 234 struct attr* newAddrAttr (unsigned short attr, struct in_addr value) {
 235
 236  struct in_addr *v=malloc(sizeof(struct in_addr));
 237
 238  *v=value;
 239
 240  return newAttr(attr, sizeof(struct in_addr), v);
 241
 242 }
 243
 244
 245
 246 // ----------------------------
 247 void freeAttr (struct attr *at) {
 248
 249  if ( at!=NULL ) {
 250   free(at->data);
 251   free(at);
 252  }
 253
 254 }
 255
 256
 257
 258 // --------------------------------------------------------------------------------------------------------------
 259 int extractPacketAttributes (struct ng_packet *np, unsigned char *error, unsigned short *attr_error, List *attr) {
 260
 261  struct attr *at;
 262  int ret=0;
 263
 264
 265  if ( error!=NULL ) *error=np->nh->error;
 266  if ( attr_error!=NULL ) *attr_error=ntohs(np->nh->attr);
 267
 268  while ( getPacketTotalSize(np)<np->maxlen ) {
 269
 270   // no room for an attribute header: error
 271   if ( getPacketTotalSize(np)+(int)sizeof(struct attr_header)>np->maxlen ) {
 272    ret=-1;
 273    break;
 274   }
 275
 276   at=malloc(sizeof(struct attr));
 277   at->attr=ntohs(np->ah->attr);
 278   at->size=ntohs(np->ah->size);
 279
 280   // attribute data bigger than the remaining size: error
 281   if ( getPacketTotalSize(np)+(int)sizeof(struct attr_header)+at->size>np->maxlen ) {
 282    free(at);
 283    ret=-1;
 284    break;
 285   }
 286
 287   if ( at->size==0 ) {
 288    at->data=NULL;
 289   } else {
 290    at->data=malloc(at->size*sizeof(char));
 291    memcpy(at->data, np->ah->data, at->size);
 292   }
 293
 294   pushBackList(attr, at);
 295
 296   // stop on an END attribute
 297   if ( at->attr==ATTR_END ) break;
 298
 299   // move to next attribute
 300   np->ah=(struct attr_header*)(np->ah->data+at->size);
 301
 302  }
 303
 304
 305  return ret;
 306
 307 }
 308
 309
 310
 311 // --------------------------------------------------------------
 312 void extractSwitchAttributes (struct swi_attr *sa, const List *l) {
 313
 314  const ListNode *ln;
 315  const struct attr *at;
 316  int len;
 317
 318
 319  memset(sa, 0, sizeof(struct swi_attr));
 320
 321  for (ln=l->first; ln!=NULL; ln=ln->next) {
 322   at=ln->data;
 323
 324   switch ( at->attr ) {
 325
 326    case ATTR_PRODUCT:
 327     len=min(at->size, PRODUCT_SIZE);
 328     memcpy(sa->product, at->data, len);
 329     trim(sa->product, len);
 330    break;
 331
 332    case ATTR_NAME:
 333     len=min(at->size, NAME_SIZE);
 334     memcpy(sa->name, at->data, len);
 335     trim(sa->name, len);
 336    break;
 337
 338    case ATTR_MAC:
 339     memcpy(&sa->mac, at->data, ETH_ALEN);
 340    break;
 341
 342    case ATTR_IP:
 343     sa->nc.ip=*(struct in_addr*)at->data;
 344    break;
 345
 346    case ATTR_NETMASK:
 347     sa->nc.netmask=*(struct in_addr*)at->data;
 348    break;
 349
 350    case ATTR_GATEWAY:
 351     sa->nc.gw=*(struct in_addr*)at->data;
 352    break;
 353
 354    case ATTR_DHCP:
 355     sa->nc.dhcp=( ntohs(*(unsigned short*)at->data)==1 );
 356    break;
 357
 358    case ATTR_FIRM_VER:
 359     len=min(at->size, FIRMWARE_SIZE-1);
 360     memcpy(sa->firmware, at->data, len);
 361     sa->firmware[len]=0;
 362    break;
 363
 364    case ATTR_PORTS_COUNT:
 365     sa->ports=*(unsigned char*)at->data;
 366    break;
 367
 368    case ATTR_END:
 369     return;
 370
 371   }
 372
 373  }
 374
 375
 376 }
 377
 378
 379