Linux HLSW LAN Master
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

144 lines
2.9 KiB

  1. #include <stdio.h>
  2. #define __USE_GNU
  3. #include <string.h>
  4. #include <ctype.h>
  5. #include <sys/socket.h>
  6. #include <netinet/in.h>
  7. #include <netinet/ip.h>
  8. #include <arpa/inet.h>
  9. #include "netpkt.h"
  10. int pkt_memcmp(struct net_pkt *pkt, unsigned int offset, char *search, unsigned int size)
  11. {
  12. if (offset >= pkt->size)
  13. return 1;
  14. /* nicht ueber das paket hinaus vergleichen */
  15. if (offset + size >= pkt->size)
  16. size = pkt->size - offset;
  17. return memcmp(pkt->buf + offset, search, size);
  18. }
  19. int pkt_memmem(struct net_pkt *pkt, unsigned int offset, char *search, unsigned int size)
  20. {
  21. void *found;
  22. if (offset >= pkt->size)
  23. return -1;
  24. found = memmem(pkt->buf + offset, pkt->size, search, size);
  25. return (found == NULL) ? -1 : (found - (void *)pkt->buf);
  26. }
  27. struct net_pkt * pkt_merge(struct net_pkt *pkt1, struct net_pkt *pkt2)
  28. {
  29. struct net_pkt *ret;
  30. ret = malloc(sizeof(struct net_pkt) + pkt1->size + pkt2->size);
  31. memcpy(&ret->addr, &pkt1->addr, sizeof(ret->addr));
  32. ret->size = pkt1->size + pkt2->size;
  33. memcpy(ret->buf, pkt1->buf, pkt1->size);
  34. memcpy(ret->buf + pkt1->size, pkt2->buf, pkt2->size);
  35. return ret;
  36. }
  37. char * pkt_ntoa(struct net_pkt *pkt)
  38. {
  39. return inet_ntoa(pkt->addr.sin_addr);
  40. }
  41. unsigned short pkt_getport(struct net_pkt *pkt)
  42. {
  43. return ntohs(pkt->addr.sin_port);
  44. }
  45. int pkt_sameaddr(struct net_pkt *pkt1, struct net_pkt *pkt2)
  46. {
  47. return (pkt1->addr.sin_addr.s_addr == pkt2->addr.sin_addr.s_addr) &&
  48. (pkt1->addr.sin_port == pkt2->addr.sin_port);
  49. }
  50. int pkt_parse_int(struct net_pkt *pkt, unsigned int offset, int *val)
  51. {
  52. unsigned char *max = pkt->buf + pkt->size;
  53. unsigned char *c = pkt->buf + offset;
  54. /* untere grenze abtesten */
  55. if (pkt->buf > c || c > max)
  56. return -1;
  57. *val = 0;
  58. /* ziffern einlesen */
  59. while (isdigit(*c) && c < max)
  60. *val = (*val * 10) + (*c++ - 0x30);
  61. return (c - (pkt->buf + offset));
  62. }
  63. int pkt_parse_ip(struct net_pkt *pkt, int offset, struct in_addr *ip)
  64. {
  65. int i, tmp, count, pos = offset;
  66. ip->s_addr = 0;
  67. for (i = 0; i < 4; i++) {
  68. count = pkt_parse_int(pkt, pos, &tmp);
  69. pos += count;
  70. if (count == 0 || tmp < 0 || tmp > 255)
  71. return 0;
  72. ip->s_addr = ip->s_addr>>8 | tmp<<24;
  73. if (i != 3 && pkt->buf[pos++] != '.')
  74. return 0;
  75. }
  76. return pos - offset;
  77. }
  78. char * pkt_print(struct net_pkt *pkt)
  79. {
  80. int pos = 0, i = 0, j;
  81. char *buf = malloc(pkt->size * 4 + 64);
  82. while (pos < pkt->size) {
  83. i += sprintf(buf + i, "%04X: ", pos);
  84. for (j = 0; j < 16; j++) {
  85. if (pos + j < pkt->size)
  86. i += sprintf(buf + i, "%02X", pkt->buf[pos + j]);
  87. else
  88. i += sprintf(buf + i, " ");
  89. if (j % 2)
  90. buf[i++] = ' ';
  91. }
  92. for (j = 0; j < 16; j++) {
  93. if (pos + j < pkt->size) {
  94. unsigned char val = pkt->buf[pos + j];
  95. if (val >= 0x20 && val < 0x80)
  96. buf[i++] = val;
  97. else
  98. buf[i++] = '.';
  99. } else {
  100. buf[i++] = ' ';
  101. }
  102. }
  103. pos += 16;
  104. buf[i++] = '\r';
  105. buf[i++] = '\n';
  106. }
  107. buf[i] = 0;
  108. return buf;
  109. }