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 = toUInt4(&data[1]);
 224                         p->errorDist = 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         if (data[2] == 0) {
 293                 return data[2];
 294         }
 295 }
 296 int gs105e_act() {
 297         gs105e_query();
 298         return gs105e_actRecv();
 299 }
 300
 301
 302 int gs105e_addVlan(int vlanId) {
 303         makeHeader(QR_EXEC);
 304
 305         char data[4] = {vlanId / 256, vlanId % 256, 0, 0};
 306
 307         addActData(ACT_ADDVLAN, 4, data);
 308
 309         return gs105e_act();
 310
 311 }
 312
 313 int gs105e_setVlanMembers(unsigned int vlanId, unsigned int members, unsigned int tagged) {
 314         char data[4] = {0, vlanId & 0xFF, (char)members & 0xFF, (char)tagged & 0xFF};
 315         makeHeader(QR_EXEC);
 316         addActData(GS_VLAN, 4, data);
 317         return gs105e_act();
 318
 319 }
 320
 321
 322 int gs105e_delVlan(int vlanId) {
 323         makeHeader(QR_EXEC);
 324
 325         char data[2] = {vlanId / 256, vlanId % 256};
 326
 327         addActData(ACT_DELVLAN, 2, data);
 328
 329         return gs105e_act();
 330
 331 }
 332 int gs105e_restart(int vlanId) {
 333         makeHeader(QR_EXEC);
 334
 335         char data[1] = {1};
 336
 337         addActData(ACT_RESTART, 1, data);
 338
 339         return gs105e_act();
 340
 341 }
 342
 343 int gs105e_mirrorPorts(int outputPort, int mirrorMask) {
 344         makeHeader(QR_EXEC);
 345         char data[3] = {outputPort, 0,  mirrorMask};
 346
 347         addActData(ACT_PORTMIRROR, 3, data);
 348         return gs105e_act();
 349 }
 350
 351 int gs105e_cableDiagnostics(int port) {
 352         makeHeader(QR_EXEC);
 353         char data[1] = {port};
 354
 355         addActData(ACT_PORTMIRROR, 3, data);
 356         return gs105e_act();
 357 }
 358
 359 int gs105e_setName(char * data) {
 360         makeHeader(QR_EXEC);
 361
 362
 363         addActData(GS_NAME, strlen(data), data);
 364         return gs105e_act();
 365 }
 366
 367 int gs105e_dhcpSettings(int action) {
 368         makeHeader(QR_EXEC);
 369         char data[1] = {(char)action};
 370
 371         addActData(ACT_DHCP, 1, data);
 372         return gs105e_act();
 373 }
 374
 375 void addDiscoveredDevice(int id) {
 376
 377         struct gs105e_discovered * ddev = gs105e_devs;
 378
 379         if (ddev == NULL) {
 380                 ddev = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 381                 gs105e_devs = ddev;
 382         }else {
 383                 while (ddev != NULL) {
 384                         if (memcmp(ddev->mac, settings.mac, 6))
 385                                 continue;
 386                         ddev = ddev->next;
 387                 }
 388         }
 389
 390         if (ddev == NULL) {
 391                 struct gs105e_discovered * ddev = gs105e_devs;
 392                 gs105e_devs = (struct gs105e_discovered *)malloc(sizeof(struct gs105e_discovered));
 393                 gs105e_devs->next = ddev;
 394                 ddev = gs105e_devs;
 395         }
 396         ddev->name = settings.name;
 397         memcpy(ddev->mac, settings.mac, 6);
 398         memcpy(ddev->ip, settings.ip, 4);
 399         memcpy(ddev->gateway, settings.gateway, 4);
 400         ddev->model = settings.model;
 401         ddev->id = id;
 402
 403 }
 404
 405
 406 int gs105e_discover(void) {
 407         int n = 0;
 408         struct gs105e_discovered * ddev = gs105e_devs;
 409         struct gs105e_discovered * ddev_T;
 410         while (ddev != NULL) {
 411                 ddev_T = ddev;
 412                 ddev = ddev->next;
 413                 free(ddev_T);
 414         }
 415         gs105e_devs = NULL;
 416
 417         makeHeader(QR_REQ);
 418         addQuery(GS_MODEL);
 419         addQuery(GS_NAME);
 420         addQuery(GS_MAC);
 421         addQuery(GS_IP);
 422         addQuery(GS_SUBNETMSK);
 423         addQuery(GS_GATEWAY);
 424
 425         gs105e_query ();
 426         gs105e_query ();
 427         while (gs105e__receive() == 0) {
 428                 addDiscoveredDevice(n + 1);
 429                 n++;
 430         }
 431         return n;
 432
 433 }
 434
 435
 436 void gs105e_queryAll(void) {
 437
 438
 439
 440         if (settings.vlans != NULL) {
 441                 struct vlan * vl = settings.vlans;
 442                 struct vlan * vl_T;
 443                 while (vl != NULL) {
 444                         vl_T = vl;
 445                         vl = vl->next;
 446                         free(vl_T);
 447                 }
 448                 settings.vlans = NULL;
 449         }
 450
 451         makeHeader(QR_REQ);
 452         addQuery(GS_MODEL);
 453         addQuery(GS_NAME);
 454         addQuery(GS_MAC);
 455         addQuery(GS_IP);
 456         addQuery(GS_SUBNETMSK);
 457         addQuery(GS_GATEWAY);
 458         addQuery(GS_PACKETSTATISTICS);
 459         addQuery(GS_FIRMWAREVER);
 460         addQuery(ACT_DHCP);
 461         addQuery(GS_PORTSTATUS);
 462         addQuery(GS_VLAN);
 463
 464         gs105e_query ();
 465          gs105e_receive();
 466 }