git.sur5r.net Git - gsconf/blob - gs105e.c

   1
   2 #include "gs105e.h"
   3 #include "socket.h"
   4 #include <string.h>
   5 #include <stdio.h>
   6 #include <stdlib.h>
   7
   8 unsigned char * gs105e_queryData;
   9 unsigned int gsDataLen;
  10
  11 struct vlan * getVlanSetting(unsigned int vlanId) {
  12         struct vlan *  _vlan = settings.vlans;
  13
  14         while (_vlan != NULL) {
  15                 if (_vlan->id == vlanId)
  16                         return _vlan;
  17                 _vlan = _vlan->next;
  18         }
  19
  20         return NULL;
  21 }
  22
  23 void addVlanSetting(unsigned int vlanId, unsigned char tagged, unsigned char member) {
  24         struct vlan * old = settings.vlans;
  25         struct vlan * new = (struct vlan *) malloc (sizeof( struct vlan) );
  26         new->id = vlanId;
  27         new->tag = tagged;
  28         new->members = member;
  29         new->next = old;
  30         settings.vlans = new;
  31 }
  32
  33 void hexDump (char * data, int len) {
  34         int n;
  35         for (n = 0; n < len; n++) {
  36                 if (n % 16 == 0)
  37                         printf("\n %06X    |",n);
  38                 printf(" %02X", (int)data[n] & 0xFF);
  39         }
  40         printf("\n");
  41 }
  42 void addData(char * data, int len) {
  43         gsDataLen += len;
  44         gs105e_queryData = (char * )realloc(gs105e_queryData, sizeof(char) * gsDataLen);
  45         memcpy(&gs105e_queryData[gsDataLen - len], data, len);
  46 }
  47 void addQuery(int qid) {
  48         char queryData[] = {qid / 256, qid % 256, 0, 0};
  49         addData(queryData, 4);
  50 }
  51
  52 void addActData(int actId, int len, char * data) {
  53
  54         char actData[] = {actId / 256, actId % 256, len / 256, len % 256};
  55         addData(actData, 4);
  56
  57         addData(data, len);
  58 }
  59 void gs105e_init(void) {
  60         packetId = 0;
  61         gs105e_devs = NULL;
  62         gs105e_queryData = NULL;
  63         gsDataLen = 0;
  64         settings.vlans = NULL;
  65 }
  66
  67
  68
  69
  70
  71 char newPacketId() {
  72         packetId ++;
  73         packetId %= 256;
  74         return (char)packetId;
  75 }
  76
  77 void makeHeader(unsigned int queryType) {
  78         int n;
  79
  80
  81
  82         gs105e_queryData = (unsigned char * )realloc(gs105e_queryData, sizeof(char) * 32);
  83         gsDataLen = 32;
  84
  85
  86         for (n = 0; n < 32; n++)
  87                 gs105e_queryData[n] = 0;
  88
  89         gs105e_queryData[0] = queryType / 256;
  90         gs105e_queryData[1] = queryType % 256;
  91
  92         memcpy(&gs105e_queryData[8], myMac, 6);
  93         if (memcpy(settings.mac, "\x00\x00\x00\x00\x00\x00", 6))
  94                 memcpy(&gs105e_queryData[14], settings.mac, 6);
  95
  96         memcpy(&gs105e_queryData[24], "\x4e\x53\x44\x50", 4); //Magic!! :-O
  97         if (settings.password != NULL && queryType == QR_EXEC) {
  98
  99                 addActData(GS_PASSWORD, strlen(settings.password), settings.password);
 100         }
 101
 102
 103
 104 }
 105
 106
 107 void debug(void) {
 108         hexDump(gs105e_queryData, gsDataLen);
 109 }
 110
 111 void emptyBuffer(void) {
 112         char tmp[2500];
 113         while (recvBroadcast(tmp) > 0 ) {
 114                 ;;
 115         }
 116 }
 117
 118 void gs105e_query (void) {
 119         emptyBuffer();
 120         gs105e_queryData[23] = newPacketId();
 121
 122         if (gs105e_queryData[gsDataLen - 4] != 0xFF | gs105e_queryData[gsDataLen - 3] != 0xFF | gs105e_queryData[gsDataLen - 2] != 0x00 | gs105e_queryData[gsDataLen - 1] != 0x00)
 123                 addData("\xFF\xFF\x00\x00", 4);
 124         sendBroadcast(gs105e_queryData, gsDataLen);
 125
 126 }
 127
 128 unsigned long toLong(char * data) {
 129         int n;
 130         unsigned long a;
 131         for (n = 0; n < 8; n++) {
 132                 a <<= 8;
 133                 a |= ((unsigned long)data[n]) & 0xFF;
 134         }
 135         return a;
 136 }
 137
 138 unsigned int toUInt4(char * data) {
 139         int n;
 140         unsigned int a;
 141         for (n = 0; n < 4; n++) {
 142                 a <<= 8;
 143                 a |= ((unsigned int )data[n]) & 0xFF;
 144         }
 145         return a;
 146 }
 147
 148 unsigned int toUInt(char * data) {
 149         int n;
 150         unsigned int a;
 151         for (n = 0; n < 2; n++) {
 152                 a <<= 8;
 153                 a |= ((unsigned int )data[n]) & 0xFF;
 154         }
 155         return a;
 156 }
 157
 158 void gs105e_interpret_slice(unsigned int ID, char * data, int len) {
 159         int mLen , tmp;
 160         struct portStatistic *p;
 161         switch (ID) {
 162                 case GS_MODEL:;
 163
 164                         mLen = (strstr(data, " ") - data) + 1;
 165                         settings.model = realloc(settings.model, sizeof(char) * mLen);
 166                         memcpy(settings.model, data, mLen);
 167                         break;
 168                 case GS_NAME:;
 169                         mLen= (strchr(data, 0x00) - data) + 1;
 170                         settings.name = realloc(settings.name, sizeof(char) * mLen);
 171                         memcpy(settings.name, data, mLen);
 172                         break;
 173                 case GS_MAC:;
 174                         memcpy(settings.mac, data, 6);
 175                         break;
 176                 case GS_IP:;
 177                         memcpy(settings.ip, data, 4);
 178                         break;
 179                 case GS_SUBNETMSK:;
 180                         memcpy(settings.subnetmask, data, 4);
 181                         break;
 182                 case GS_GATEWAY:;
 183                         memcpy(settings.gateway, data, 4);
 184                         break;
 185                 case GS_FIRMWAREVER:;
 186                         settings.swVersion = realloc(settings.swVersion, sizeof(char) * len);
 187                         memcpy(settings.swVersion, data, len);
 188                         break;
 189                 case GS_PACKETSTATISTICS:;
 190                         tmp = (unsigned int)data[0] - 1;
 191                         if (tmp > 5)
 192                                 return;
 193                         p = &settings.portStatistics[tmp];
 194                         p->portId = tmp + 1;
 195                         p->bytesIn = toLong(&data[1]);
 196                         p->bytesOut = toLong(&data[9]);
 197                         p->errors = toLong(&data[41]);
 198                         break;
 199                 case GS_VLAN:;
 200                         unsigned int vlanId = toUInt(data);
 201                         unsigned char tagged = (unsigned char) data[3];
 202                         unsigned char memberOf = (unsigned char) data[2];
 203                         struct vlan * _vlan = getVlanSetting(vlanId);
 204
 205                         if (_vlan == NULL) {
 206                                 addVlanSetting(vlanId, tagged, memberOf);
 207                         }else {
 208                              _vlan->members = memberOf;
 209                              _vlan->tag = tagged;
 210
 211                         }
 212
 213
 214                         break;
 215                 case GS_PORTSTATUS:;
 216                         unsigned int portId = (unsigned int) data[0];
 217                         unsigned char state = (unsigned char) data[1];
 218                         p = &settings.portStatistics[portId - 1];
 219                         p->state = state;
 220                         break;
 221                 case GS_PORTDIAG:;
 222                         p = &settings.portStatistics[data[0]];
 223                         p->cableError = (char)toUInt4(&data[1]);
 224                         p->errorDist = (char)toUInt4(&data[5]);
 225                         break;
 226                 case ACT_DHCP:;
 227                         settings.dhcp = data[1] & 0x03;
 228                         break;
 229         }
 230 }
 231
 232 int gs105e__receive(void) {
 233         char data[2500];
 234         int len = recvBroadcast(data);
 235         int n;
 236
 237         unsigned int id;
 238         unsigned int slLen;
 239
 240
 241 //        if (len > 0)
 242 //                hexDump(data, len);
 243         if (len == -1) {
 244 //                printf("Received No or Invalid Packet\n");
 245                 return -1;
 246         }
 247         if (memcmp(&data[8], myMac, 6) | data[0] != 0x01 | (!(data[1] == 0x02 | data[1] == 0x04))) {
 248                 return -1;
 249         }
 250
 251         for (n = 32; n < len; ) {
 252                id = (data[n] * 256)  + data[n + 1];
 253                slLen = (data[n+2] * 256)  + data[n + 3];
 254
 255                gs105e_interpret_slice(id, &data[n + 4], slLen);
 256                n += slLen + 4;
 257         }
 258         return 0;
 259
 260
 261 }
 262
 263 int gs105e_receive(void) {
 264         if (gs105e__receive() == -1) {
 265                 gs105e_query();
 266                 return gs105e_receive();
 267         }
 268         return 0;
 269 }
 270
 271
 272 int gs105e_actRecv(void) {
 273         char data[2500];
 274         int len = recvBroadcast_tout(data);
 275         int n;
 276
 277         unsigned int id;
 278         unsigned int slLen;
 279
 280
 281 //        if (len > 0)
 282 //                hexDump(data, len);
 283         if (len == -1) {
 284 //                printf("Received No or Invalid Packet\n");
 285                 return -1;
 286         }
 287         if (memcmp(&data[8], myMac, 6) | data[0] != 0x01 | (!(data[1] == 0x02 | data[1] == 0x04))) {
 288                 gs105e_actRecv();
 289                 return -1;
 290         }
 291
 292
 293
 294         if (data[2] == 0) {
 295                 for (n = 32; n < len; ) {
 296                         id = (data[n] * 256)  + data[n + 1];
 297                         slLen = (data[n+2] * 256)  + data[n + 3];
 298
 299                         gs105e_interpret_slice(id, &data[n + 4], slLen);
 300                         n += slLen + 4;
 301                 }
 302                 return data[2];
 303         }
 304 }
 305 int gs105e_act() {
 306         gs105e_query();
 307         return gs105e_actRecv();
 308 }
 309
 310
 311 int gs105e_addVlan(int vlanId) {
 312         makeHeader(QR_EXEC);
 313
 314         char data[4] = {vlanId / 256, vlanId % 256, 0, 0};
 315
 316         addActData(ACT_ADDVLAN, 4, data);
 317
 318         return gs105e_act();
 319
 320 }
 321
 322 int gs105e_setVlanMembers(unsigned int vlanId, unsigned int members, unsigned int tagged) {
 323         char data[4] = {0, vlanId & 0xFF, (char)members & 0xFF, (char)tagged & 0xFF};
 324         makeHeader(QR_EXEC);
 325         addActData(GS_VLAN, 4, data);
 326         return gs105e_act();
 327
 328 }
 329
 330
 331 int gs105e_delVlan(int vlanId) {
 332         makeHeader(QR_EXEC);
 333
 334         char data[2] = {vlanId / 256, vlanId % 256};
 335
 336         addActData(ACT_DELVLAN, 2, data);
 337
 338         return gs105e_act();
 339
 340 }
 341 int gs105e_restart(int vlanId) {
 342         makeHeader(QR_EXEC);
 343
 344         char data[1] = {1};
 345
 346         addActData(ACT_RESTART, 1, data);
 347
 348         return gs105e_act();
 349
 350 }
 351
 352 int gs105e_mirrorPorts(int outputPort, int mirrorMask) {
 353         makeHeader(QR_EXEC);
 354         char data[3] = {outputPort, 0,  mirrorMask};
 355
 356         addActData(ACT_PORTMIRROR, 3, data);
 357         return gs105e_act();
 358 }
 359
 360 int gs105e_cableDiagnostics(int port) {
 361         makeHeader(QR_REQ);
 362         char data[1] = {port};
 363
 364         addActData(GS_PORTDIAG, 1, data);
 365         return gs105e_act();
 366 }
 367
 368 int gs105e_setName(char * data) {
 369         makeHeader(QR_EXEC);
 370
 371
 372         addActData(GS_NAME, strlen(data), data);
 373         return gs105e_act();
 374 }
 375
 376 int gs105e_dhcpSettings(int action) {
 377         makeHeader(QR_EXEC);
 378         char data[1] = {(char)action};
 379
 380         addActData(ACT_DHCP, 1, data);
 381         return gs105e_act();
 382 }
 383
 384 void addDiscoveredDevice(int id) {
 385
 386         struct gs105e_discovered * ddev = gs105e_devs;
 387
 388         if (ddev == NULL) {
 389                 ddev = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 390                 gs105e_devs = ddev;
 391         }else {
 392                 while (ddev != NULL) {
 393                         if (memcmp(ddev->mac, settings.mac, 6))
 394                                 continue;
 395                         ddev = ddev->next;
 396                 }
 397         }
 398
 399         if (ddev == NULL) {
 400                 struct gs105e_discovered * ddev = gs105e_devs;
 401                 gs105e_devs = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 402                 gs105e_devs->next = ddev;
 403                 ddev = gs105e_devs;
 404         }
 405         ddev->name = settings.name;
 406         memcpy(ddev->mac, settings.mac, 6);
 407         memcpy(ddev->ip, settings.ip, 4);
 408         memcpy(ddev->gateway, settings.gateway, 4);
 409         ddev->model = settings.model;
 410         ddev->id = id;
 411
 412 }
 413
 414
 415 int gs105e_discover(void) {
 416         int n = 0;
 417         struct gs105e_discovered * ddev = gs105e_devs;
 418         struct gs105e_discovered * ddev_T;
 419         while (ddev != NULL) {
 420                 ddev_T = ddev;
 421                 ddev = ddev->next;
 422                 free(ddev_T);
 423         }
 424         gs105e_devs = NULL;
 425
 426         makeHeader(QR_REQ);
 427         addQuery(GS_MODEL);
 428         addQuery(GS_NAME);
 429         addQuery(GS_MAC);
 430         addQuery(GS_IP);
 431         addQuery(GS_SUBNETMSK);
 432         addQuery(GS_GATEWAY);
 433
 434         gs105e_query ();
 435         gs105e_query ();
 436         while (gs105e__receive() == 0) {
 437                 addDiscoveredDevice(n + 1);
 438                 n++;
 439         }
 440         return n;
 441
 442 }
 443
 444
 445 void gs105e_queryAll(void) {
 446
 447
 448
 449         if (settings.vlans != NULL) {
 450                 struct vlan * vl = settings.vlans;
 451                 struct vlan * vl_T;
 452                 while (vl != NULL) {
 453                         vl_T = vl;
 454                         vl = vl->next;
 455                         free(vl_T);
 456                 }
 457                 settings.vlans = NULL;
 458         }
 459
 460         makeHeader(QR_REQ);
 461         addQuery(GS_MODEL);
 462         addQuery(GS_NAME);
 463         addQuery(GS_MAC);
 464         addQuery(GS_IP);
 465         addQuery(GS_SUBNETMSK);
 466         addQuery(GS_GATEWAY);
 467         addQuery(GS_PACKETSTATISTICS);
 468         addQuery(GS_FIRMWAREVER);
 469         addQuery(ACT_DHCP);
 470         addQuery(GS_PORTSTATUS);
 471         addQuery(GS_VLAN);
 472
 473         gs105e_query ();
 474          gs105e_receive();
 475 }