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

   1
   2 #include "protocol.h"
   3
   4
   5
   6 const unsigned short helloRequest[]={
   7  ATTR_PRODUCT,
   8  ATTR_UNK2,
   9  ATTR_NAME,
  10  ATTR_MAC,
  11  ATTR_UNK5,
  12  ATTR_IP,
  13  ATTR_NETMASK,
  14  ATTR_GATEWAY,
  15  ATTR_DHCP,
  16  ATTR_UNK12,
  17  ATTR_FIRM_VER,
  18  ATTR_UNK14,
  19  ATTR_UNK15,
  20  ATTR_END
  21 };
  22
  23
  24 const struct ether_addr nullMac={.ether_addr_octet={0, 0, 0, 0, 0, 0}};
  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->unk1=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  int i;
  61
  62
  63  if ( nh->unk1!=1 ) {
  64   //printf("unk1 not 1\n");
  65   return false;
  66  }
  67
  68  if ( code>0 && nh->code!=code ) {
  69   return false;
  70  }
  71
  72  for (i=0; i<6; i++) {
  73   if ( nh->unk2[i]!=0 ) {
  74    //printf("unk2[%i] not 0\n", i);
  75    return false;
  76   }
  77  }
  78
  79  if ( client_mac!=NULL && memcmp(nh->client_mac, client_mac, ETH_ALEN)!=0 ) {
  80   //printf("client_mac err\n");
  81   return false;
  82  }
  83
  84  if ( switch_mac!=NULL && memcmp(nh->switch_mac, switch_mac, ETH_ALEN)!=0 ) {
  85   //printf("switch_mac err\n");
  86   return false;
  87  }
  88
  89  if ( seqnum>0 && ntohl(nh->seqnum)!=seqnum ) {
  90   //printf("incorrect seqnum\n");
  91   return false;
  92  }
  93
  94  if ( *(unsigned int*)nh->unk3!=0 ) {
  95   //printf("unk3 not 0\n");
  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 struct attr* newEmptyAttr (unsigned short attr) {
 172
 173  struct attr *at;
 174
 175
 176  at=malloc(sizeof(struct attr));
 177  at->attr=attr;
 178  at->size=0;
 179  at->data=NULL;
 180
 181
 182  return at;
 183
 184 }
 185
 186
 187 // ----------------------------
 188 void freeAttr (struct attr *at) {
 189
 190  if ( at!=NULL ) {
 191   free(at->data);
 192   free(at);
 193  }
 194
 195 }
 196
 197
 198
 199 // -------------------------------------------------
 200 List* extractPacketAttributes (struct ng_packet *np) {
 201
 202  List *l;
 203  struct attr *at;
 204
 205
 206  l=createEmptyList();
 207
 208  while ( getPacketTotalSize(np)<np->maxlen ) {
 209
 210   at=malloc(sizeof(struct attr));
 211   at->attr=ntohs(np->ah->attr);
 212   at->size=ntohs(np->ah->size);
 213
 214   if ( getPacketTotalSize(np)+at->size>np->maxlen ) {
 215    free(at);
 216    break;
 217   }
 218
 219   if ( at->size==0 ) {
 220    at->data=NULL;
 221   } else {
 222    at->data=malloc(at->size*sizeof(char));
 223    memcpy(at->data, np->ah->data, at->size);
 224   }
 225
 226   pushBackList(l, at);
 227
 228   if ( at->attr==ATTR_END ) {
 229    break;
 230   }
 231
 232   np->ah=(struct attr_header*)(np->ah->data+at->size);
 233
 234  }
 235
 236
 237  return l;
 238
 239 }
 240
 241