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 (memcmp(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                 case GS_VLANSETTING:;
 230                         settings.vlanType = data[0];
 231                         break;
 232         }
 233 }
 234
 235 int gs105e__receive(void) {
 236         char data[2500];
 237         int len = recvBroadcast(data);
 238         int n;
 239
 240         unsigned int id;
 241         unsigned int slLen;
 242
 243
 244         if (len > 0)
 245                 hexDump(data, len);
 246         if (len == -1) {
 247                 printf("Received No or Invalid Packet\n");
 248                 return -1;
 249         }
 250         if (memcmp(&data[8], myMac, 6) | data[0] != 0x01 | (!(data[1] == 0x02 | data[1] == 0x04))) {
 251                 return -1;
 252         }
 253
 254         for (n = 32; n < len; ) {
 255                id = (data[n] * 256)  + data[n + 1];
 256                slLen = (data[n+2] * 256)  + data[n + 3];
 257
 258                gs105e_interpret_slice(id, &data[n + 4], slLen);
 259                n += slLen + 4;
 260         }
 261         return 0;
 262
 263
 264 }
 265
 266 int gs105e_receive(void) {
 267         if (gs105e__receive() == -1) {
 268                 gs105e_query();
 269                 return gs105e_receive();
 270         }
 271         return 0;
 272 }
 273
 274
 275 int gs105e_actRecv(void) {
 276         char data[2500];
 277         int len = recvBroadcast_tout(data);
 278         int n;
 279
 280         unsigned int id;
 281         unsigned int slLen;
 282
 283
 284 //        if (len > 0)
 285 //                hexDump(data, len);
 286         if (len == -1) {
 287 //                printf("Received No or Invalid Packet\n");
 288                 return -1;
 289         }
 290         if (memcmp(&data[8], myMac, 6) | data[0] != 0x01 | (!(data[1] == 0x02 | data[1] == 0x04))) {
 291                 gs105e_actRecv();
 292                 return -1;
 293         }
 294
 295
 296
 297         if (data[2] == 0) {
 298                 for (n = 32; n < len; ) {
 299                         id = (data[n] * 256)  + data[n + 1];
 300                         slLen = (data[n+2] * 256)  + data[n + 3];
 301
 302                         gs105e_interpret_slice(id, &data[n + 4], slLen);
 303                         n += slLen + 4;
 304                 }
 305                 return data[2];
 306         }
 307 }
 308 int gs105e_act() {
 309         gs105e_query();
 310         int n = gs105e_actRecv();
 311         if (n < 0){
 312                 gs105e_query();
 313                 n = gs105e_actRecv();
 314         }
 315         return n;
 316 }
 317
 318
 319 int gs105e_addVlan(int vlanId) {
 320         makeHeader(QR_EXEC);
 321
 322         char data[4] = {vlanId / 256, vlanId % 256, 0, 0};
 323
 324         addActData(ACT_ADDVLAN, 4, data);
 325
 326         return gs105e_act();
 327
 328 }
 329
 330 int gs105e_setVlanMembers(unsigned int vlanId, unsigned int members, unsigned int tagged) {
 331         char data[4] = {0, vlanId & 0xFF, (char)members & 0xFF, (char)tagged & 0xFF};
 332         makeHeader(QR_EXEC);
 333         addActData(GS_VLAN, 4, data);
 334         return gs105e_act();
 335
 336 }
 337
 338
 339 int gs105e_delVlan(int vlanId) {
 340         makeHeader(QR_EXEC);
 341
 342         char data[2] = {vlanId / 256, vlanId % 256};
 343
 344         addActData(ACT_DELVLAN, 2, data);
 345
 346         return gs105e_act();
 347
 348 }
 349 int gs105e_restart(int vlanId) {
 350         makeHeader(QR_EXEC);
 351
 352         char data[1] = {1};
 353
 354         addActData(ACT_RESTART, 1, data);
 355
 356         return gs105e_act();
 357
 358 }
 359
 360 int gs105e_mirrorPorts(int outputPort, int mirrorMask) {
 361         makeHeader(QR_EXEC);
 362         char data[3] = {(char)outputPort, 0, (char) mirrorMask};
 363
 364         addActData(ACT_PORTMIRROR, 3, data);
 365         return gs105e_act();
 366 }
 367
 368 int gs105e_cableDiagnostics(int port) {
 369         makeHeader(QR_REQ);
 370         char data[1] = {port};
 371
 372         addActData(GS_PORTDIAG, 1, data);
 373         return gs105e_act();
 374 }
 375
 376 int gs105e_setName(char * data) {
 377         makeHeader(QR_EXEC);
 378
 379
 380         addActData(GS_NAME, strlen(data), data);
 381         return gs105e_act();
 382 }
 383
 384 int gs105e_setPassword(char * data) {
 385         makeHeader(QR_EXEC);
 386
 387
 388         addActData(GS_NEWPASSWORD, strlen(data), data);
 389         return gs105e_act();
 390 }
 391
 392 int gs105e_dhcpSettings(int action) {
 393         makeHeader(QR_EXEC);
 394         char data[1] = {(char)action};
 395
 396         addActData(ACT_DHCP, 1, data);
 397         return gs105e_act();
 398 }
 399
 400 int gs105e_vlanEnable(void) {
 401         makeHeader(QR_EXEC);
 402         char data[1] = {4};
 403
 404         addActData(GS_VLANSETTING, 1, data);
 405         return gs105e_act();
 406 }
 407
 408 void addDiscoveredDevice(int id) {
 409
 410         struct gs105e_discovered * ddev = gs105e_devs;
 411
 412         if (ddev == NULL) {
 413                 ddev = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 414                 gs105e_devs = ddev;
 415                 ddev->next = NULL;
 416         }else {
 417                 while (ddev != NULL) {
 418                         if (memcmp(ddev->mac, settings.mac, 6)) {
 419                                 ddev = ddev->next;
 420                                 continue;
 421                         }
 422
 423                         break;
 424                 }
 425
 426         }
 427
 428         if (ddev == NULL) {
 429
 430                 ddev = gs105e_devs;
 431                 gs105e_devs = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 432                 printf("%p\n", gs105e_devs);
 433                 printf("%p\n", ddev);
 434                 gs105e_devs->next = ddev;
 435                 ddev = gs105e_devs;
 436                 printf("%p\n", ddev);
 437
 438         }
 439
 440         printf("%p\n", ddev);
 441         ddev->name = (char *)realloc(ddev->name, sizeof(char) * strlen(settings.name));
 442         printf("DEBUG 1\n");
 443         memcpy(ddev->name, settings.name, strlen(settings.name) + 1);
 444         memcpy(ddev->mac, settings.mac, 6);
 445
 446         memcpy(ddev->ip, settings.ip, 4);
 447         memcpy(ddev->gateway, settings.gateway, 4);
 448         ddev->model = (char *)malloc(sizeof(char) * strlen(settings.model));
 449         memcpy(ddev->model, settings.model, strlen(settings.model) + 1);
 450
 451         ddev->id = id;
 452
 453 }
 454
 455
 456 int gs105e_discover(void) {
 457         int n = 0;
 458         struct gs105e_discovered * ddev = gs105e_devs;
 459         struct gs105e_discovered * ddev_T;
 460         while (ddev != NULL) {
 461                 ddev_T = ddev;
 462                 ddev = ddev->next;
 463                 free(ddev_T);
 464         }
 465         gs105e_devs = NULL;
 466
 467         makeHeader(QR_REQ);
 468         addQuery(GS_MODEL);
 469         addQuery(GS_NAME);
 470         addQuery(GS_MAC);
 471         addQuery(GS_IP);
 472         addQuery(GS_SUBNETMSK);
 473         addQuery(GS_GATEWAY);
 474
 475         gs105e_query ();
 476         gs105e_query ();
 477         while (gs105e__receive() == 0) {
 478                 printf("DEGUB DEVICE DISCOVERED\n");
 479                 addDiscoveredDevice(n + 1);
 480
 481                 n++;
 482         }
 483         return n;
 484
 485 }
 486
 487
 488 void gs105e_queryAll(void) {
 489
 490
 491
 492         if (settings.vlans != NULL) {
 493                 struct vlan * vl = settings.vlans;
 494                 struct vlan * vl_T;
 495                 while (vl != NULL) {
 496                         vl_T = vl;
 497                         vl = vl->next;
 498                         free(vl_T);
 499                 }
 500                 settings.vlans = NULL;
 501         }
 502
 503         makeHeader(QR_REQ);
 504         addQuery(GS_MODEL);
 505         addQuery(GS_NAME);
 506         addQuery(GS_MAC);
 507         addQuery(GS_IP);
 508         addQuery(GS_SUBNETMSK);
 509         addQuery(GS_GATEWAY);
 510         addQuery(GS_PACKETSTATISTICS);
 511         addQuery(GS_FIRMWAREVER);
 512         addQuery(ACT_DHCP);
 513         addQuery(GS_PORTSTATUS);
 514         addQuery(GS_VLAN);
 515         addQuery(GS_VLANSETTING);
 516
 517         gs105e_query ();
 518          gs105e_receive();
 519 }