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

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