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 char * gs105e_queryData;
   9 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
  78
  79 void makeHeader(unsigned int queryType) {
  80         int n;
  81
  82
  83
  84         gs105e_queryData = realloc(gs105e_queryData, sizeof(char) * 32);
  85         gsDataLen = 32;
  86
  87
  88         for (n = 0; n < 32; n++)
  89                 gs105e_queryData[n] = 0;
  90
  91         gs105e_queryData[0] = queryType / 256;
  92         gs105e_queryData[1] = queryType % 256;
  93
  94         memcpy(&gs105e_queryData[8], myMac, 6);
  95         if (memcmp(settings.mac, "\x00\x00\x00\x00\x00\x00", 6))
  96                 memcpy(&gs105e_queryData[14], settings.mac, 6);
  97
  98         memcpy(&gs105e_queryData[24], "\x4e\x53\x44\x50", 4); //Magic!! :-O
  99         if (settings.password != NULL && queryType == QR_EXEC) {
 100                 char tmpPassword[strlen(settings.password)];
 101                 for (n = 0; n < strlen(settings.password); n++)
 102                         tmpPassword[n] = passwordSecret[n % 19] ^ settings.password[n];
 103                 addActData(GS_PASSWORD, strlen(settings.password), tmpPassword);
 104         }
 105
 106
 107
 108 }
 109
 110
 111 void debug(void) {
 112         hexDump(gs105e_queryData, gsDataLen);
 113 }
 114
 115 void emptyBuffer(void) {
 116         char tmp[2500];
 117         while (recvBroadcast(tmp) > 0 ) {
 118                 ;;
 119         }
 120 }
 121
 122 void gs105e_query (void) {
 123         emptyBuffer();
 124         gs105e_queryData[23] = newPacketId();
 125
 126         if ((gs105e_queryData[gsDataLen - 4] != 0xFF) | (gs105e_queryData[gsDataLen - 3] != 0xFF) | (gs105e_queryData[gsDataLen - 2] != 0x00) | (gs105e_queryData[gsDataLen - 1] != 0x00))
 127                 addData("\xFF\xFF\x00\x00", 4);
 128         sendBroadcast(gs105e_queryData, gsDataLen);
 129
 130 }
 131
 132 unsigned long toLong(char * data) {
 133         int n;
 134         unsigned long a;
 135         for (n = 0; n < 8; n++) {
 136                 a <<= 8;
 137                 a |= ((unsigned long)data[n]) & 0xFF;
 138         }
 139         return a;
 140 }
 141
 142 unsigned int toUInt4(char * data) {
 143         int n;
 144         unsigned int a;
 145         for (n = 0; n < 4; n++) {
 146                 a <<= 8;
 147                 a |= ((unsigned int )data[n]) & 0xFF;
 148         }
 149         return a;
 150 }
 151
 152 unsigned int toUInt(char * data) {
 153         int n;
 154         unsigned int a;
 155         for (n = 0; n < 2; n++) {
 156                 a <<= 8;
 157                 a |= ((unsigned int )data[n]) & 0xFF;
 158         }
 159         return a;
 160 }
 161
 162 void gs105e_interpret_slice(unsigned int ID, char * data, int len) {
 163         int mLen , tmp;
 164         struct portStatistic *p;
 165         switch (ID) {
 166                 case GS_MODEL:;
 167
 168                         mLen = (strstr(data, " ") - data) + 1;
 169                         settings.model = realloc(settings.model, sizeof(char) * mLen);
 170                         memcpy(settings.model, data, mLen);
 171                         break;
 172                 case GS_NAME:;
 173                         mLen= (strchr(data, 0x00) - data) + 1;
 174                         settings.name = realloc(settings.name, sizeof(char) * mLen);
 175                         memcpy(settings.name, data, mLen);
 176                         break;
 177                 case GS_MAC:;
 178                         memcpy(settings.mac, data, 6);
 179                         break;
 180                 case GS_IP:;
 181                         memcpy(settings.ip, data, 4);
 182                         break;
 183                 case GS_SUBNETMSK:;
 184                         memcpy(settings.subnetmask, data, 4);
 185                         break;
 186                 case GS_GATEWAY:;
 187                         memcpy(settings.gateway, data, 4);
 188                         break;
 189                 case GS_FIRMWAREVER:;
 190                         settings.swVersion = realloc(settings.swVersion, sizeof(char) * len);
 191                         memcpy(settings.swVersion, data, len);
 192                         break;
 193                 case GS_PACKETSTATISTICS:;
 194                         tmp = (unsigned int)data[0] - 1;
 195                         if (tmp > 5)
 196                                 return;
 197                         p = &settings.portStatistics[tmp];
 198                         p->portId = tmp + 1;
 199                         p->bytesIn = toLong(&data[1]);
 200                         p->bytesOut = toLong(&data[9]);
 201                         p->errors = toLong(&data[41]);
 202                         break;
 203                 case GS_VLAN:;
 204                         unsigned int vlanId = toUInt(data);
 205                         unsigned char tagged = (unsigned char) data[3];
 206                         unsigned char memberOf = (unsigned char) data[2];
 207                         struct vlan * _vlan = getVlanSetting(vlanId);
 208
 209                         if (_vlan == NULL) {
 210                                 addVlanSetting(vlanId, tagged, memberOf);
 211                         }else {
 212                              _vlan->members = memberOf;
 213                              _vlan->tag = tagged;
 214
 215                         }
 216
 217
 218                         break;
 219                 case GS_PORTSTATUS:;
 220                         unsigned int portId = (unsigned int) data[0];
 221                         unsigned char state = (unsigned char) data[1];
 222                         p = &settings.portStatistics[portId - 1];
 223                         p->state = state;
 224                         break;
 225                 case GS_PORTDIAG:;
 226                         p = &settings.portStatistics[data[0]];
 227                         p->cableError = (char)toUInt4(&data[1]);
 228                         p->errorDist = (char)toUInt4(&data[5]);
 229                         break;
 230                 case ACT_DHCP:;
 231                         settings.dhcp = data[1] & 0x03;
 232                         break;
 233                 case GS_VLANSETTING:;
 234                         settings.vlanType = data[0];
 235                         break;
 236         }
 237 }
 238
 239 int gs105e__receive(void) {
 240         char data[2500];
 241         int len = recvBroadcast(data);
 242         int n;
 243
 244         unsigned int id;
 245         unsigned int slLen;
 246
 247
 248 //        if (len > 0)
 249 //                hexDump(data, len);
 250         if (len == -1) {
 251 //                printf("Received No or Invalid Packet\n");
 252                 return -1;
 253         }
 254         if (memcmp(&data[8], myMac, 6) | (data[0] != 0x01) | (!((data[1] == 0x02) | (data[1] == 0x04)))) {
 255                 return -1;
 256         }
 257
 258         for (n = 32; n < len; ) {
 259                id = (data[n] * 256)  + data[n + 1];
 260                slLen = (data[n+2] * 256)  + data[n + 3];
 261
 262                gs105e_interpret_slice(id, &data[n + 4], slLen);
 263                n += slLen + 4;
 264         }
 265         return 0;
 266
 267
 268 }
 269
 270 int gs105e_receive(void) {
 271         if (gs105e__receive() == -1) {
 272                 gs105e_query();
 273                 return gs105e_receive();
 274         }
 275         return 0;
 276 }
 277
 278
 279 int gs105e_actRecv(void) {
 280         char data[2500];
 281         int len = recvBroadcast_tout(data);
 282         int n;
 283
 284         unsigned int id;
 285         unsigned int slLen;
 286
 287
 288 //        if (len > 0)
 289 //                hexDump(data, len);
 290         if (len == -1) {
 291 //                printf("Received No or Invalid Packet\n");
 292                 return -1;
 293         }
 294         if (memcmp(&data[8], myMac, 6) | (data[0] != 0x01) | (!((data[1] == 0x02) | (data[1] == 0x04)))) {
 295                 gs105e_actRecv();
 296                 return -1;
 297         }
 298
 299
 300
 301         if (data[2] == 0) {
 302                 for (n = 32; n < len; ) {
 303                         id = (data[n] * 256)  + data[n + 1];
 304                         slLen = (data[n+2] * 256)  + data[n + 3];
 305
 306                         gs105e_interpret_slice(id, &data[n + 4], slLen);
 307                         n += slLen + 4;
 308                 }
 309                 return data[2];
 310         }
 311 }
 312 int gs105e_act() {
 313         gs105e_query();
 314         int n = gs105e_actRecv();
 315         if (n < 0){
 316                 gs105e_query();
 317                 n = gs105e_actRecv();
 318         }
 319         return n;
 320 }
 321
 322
 323 int gs105e_addVlan(int vlanId) {
 324         makeHeader(QR_EXEC);
 325
 326         char data[4] = {vlanId / 256, vlanId % 256, 0, 0};
 327
 328         addActData(ACT_ADDVLAN, 4, data);
 329
 330         return gs105e_act();
 331
 332 }
 333
 334 int gs105e_setVlanMembers(unsigned int vlanId, unsigned int members, unsigned int tagged) {
 335         char data[4] = {0, vlanId & 0xFF, (char)members & 0xFF, (char)tagged & 0xFF};
 336         makeHeader(QR_EXEC);
 337         addActData(GS_VLAN, 4, data);
 338         return gs105e_act();
 339
 340 }
 341
 342
 343 int gs105e_delVlan(int vlanId) {
 344         makeHeader(QR_EXEC);
 345
 346         char data[2] = {vlanId / 256, vlanId % 256};
 347
 348         addActData(ACT_DELVLAN, 2, data);
 349
 350         return gs105e_act();
 351
 352 }
 353 int gs105e_restart(void) {
 354         makeHeader(QR_EXEC);
 355
 356         char data[1] = {1};
 357
 358         addActData(ACT_RESTART, 1, data);
 359
 360         return gs105e_act();
 361
 362 }
 363
 364 int gs105e_mirrorPorts(int outputPort, int mirrorMask) {
 365         makeHeader(QR_EXEC);
 366         char data[3] = {(char)outputPort, 0, (char) mirrorMask};
 367
 368         addActData(ACT_PORTMIRROR, 3, data);
 369         return gs105e_act();
 370 }
 371
 372 int gs105e_cableDiagnostics(int port) {
 373         makeHeader(QR_REQ);
 374         char data[1] = {port};
 375
 376         addActData(GS_PORTDIAG, 1, data);
 377         return gs105e_act();
 378 }
 379
 380 int gs105e_setName(char * data) {
 381         makeHeader(QR_EXEC);
 382
 383
 384         addActData(GS_NAME, strlen(data), data);
 385         return gs105e_act();
 386 }
 387
 388 int gs105e_setPassword(char * data) {
 389         int n;
 390         makeHeader(QR_EXEC);
 391
 392         char tmpPasswd[strlen(data)];
 393         for (n = 0; n < strlen(data); n++)
 394                 tmpPasswd[n] = data[n] ^ passwordSecret[n % 19];
 395         addActData(GS_NEWPASSWORD, strlen(data), tmpPasswd);
 396         return gs105e_act();
 397 }
 398
 399 int gs105e_dhcpSettings(int action) {
 400         makeHeader(QR_EXEC);
 401         char data[1] = {(char)action};
 402
 403         addActData(ACT_DHCP, 1, data);
 404         return gs105e_act();
 405 }
 406
 407 int gs105e_vlanEnable(void) {
 408         makeHeader(QR_EXEC);
 409         char data[1] = {4};
 410
 411         addActData(GS_VLANSETTING, 1, data);
 412         return gs105e_act();
 413 }
 414
 415 void addDiscoveredDevice(int id) {
 416
 417         struct gs105e_discovered * ddev = gs105e_devs;
 418
 419         if (ddev == NULL) {
 420                 ddev = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 421                 gs105e_devs = ddev;
 422                 ddev->next = NULL;
 423         }else {
 424                 while (ddev != NULL) {
 425                         if (memcmp(ddev->mac, settings.mac, 6)) {
 426                                 ddev = ddev->next;
 427                                 continue;
 428                         }
 429
 430                         break;
 431                 }
 432
 433         }
 434
 435         if (ddev == NULL) {
 436
 437                 ddev = gs105e_devs;
 438                 gs105e_devs = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 439                 printf("%p\n", gs105e_devs);
 440                 printf("%p\n", ddev);
 441                 gs105e_devs->next = ddev;
 442                 ddev = gs105e_devs;
 443                 printf("%p\n", ddev);
 444
 445         }
 446
 447         printf("%p\n", ddev->name);
 448         ddev->name = (char *)malloc(sizeof(char) * strlen(settings.name));
 449         printf("DEBUG 1\n");
 450         memcpy(ddev->name, settings.name, strlen(settings.name) + 1);
 451         memcpy(ddev->mac, settings.mac, 6);
 452
 453         memcpy(ddev->ip, settings.ip, 4);
 454         memcpy(ddev->gateway, settings.gateway, 4);
 455         ddev->model = (char *)malloc(sizeof(char) * strlen(settings.model));
 456         memcpy(ddev->model, settings.model, strlen(settings.model) + 1);
 457
 458         ddev->id = id;
 459
 460 }
 461
 462
 463 int gs105e_discover(void) {
 464         int n = 0;
 465         struct gs105e_discovered * ddev = gs105e_devs;
 466         struct gs105e_discovered * ddev_T;
 467         while (ddev != NULL) {
 468                 ddev_T = ddev;
 469                 ddev = ddev->next;
 470                 free(ddev_T);
 471         }
 472         gs105e_devs = NULL;
 473
 474         makeHeader(QR_REQ);
 475         addQuery(GS_MODEL);
 476         addQuery(GS_NAME);
 477         addQuery(GS_MAC);
 478         addQuery(GS_IP);
 479         addQuery(GS_SUBNETMSK);
 480         addQuery(GS_GATEWAY);
 481
 482         gs105e_query ();
 483         gs105e_query ();
 484         while (gs105e__receive() == 0) {
 485                 printf("DEGUB DEVICE DISCOVERED\n");
 486                 addDiscoveredDevice(n + 1);
 487
 488                 n++;
 489         }
 490         return n;
 491
 492 }
 493
 494
 495 void gs105e_queryAll(void) {
 496
 497
 498
 499         if (settings.vlans != NULL) {
 500                 struct vlan * vl = settings.vlans;
 501                 struct vlan * vl_T;
 502                 while (vl != NULL) {
 503                         vl_T = vl;
 504                         vl = vl->next;
 505                         free(vl_T);
 506                 }
 507                 settings.vlans = NULL;
 508         }
 509
 510         makeHeader(QR_REQ);
 511         addQuery(GS_MODEL);
 512         addQuery(GS_NAME);
 513         addQuery(GS_MAC);
 514         addQuery(GS_IP);
 515         addQuery(GS_SUBNETMSK);
 516         addQuery(GS_GATEWAY);
 517         addQuery(GS_PACKETSTATISTICS);
 518         addQuery(GS_FIRMWAREVER);
 519         addQuery(ACT_DHCP);
 520         addQuery(GS_PORTSTATUS);
 521         addQuery(GS_VLAN);
 522         addQuery(GS_VLANSETTING);
 523
 524         gs105e_query ();
 525          gs105e_receive();
 526 }