control.c

   1 // L2TPNS: control
   2
   3 char const *cvs_id_control = "$Id: control.c,v 1.2 2004/06/28 02:43:13 fred_nerk Exp $";
   4
   5 #include <stdio.h>
   6 #include <arpa/inet.h>
   7 #include <errno.h>
   8 #include <fcntl.h>
   9 #include <malloc.h>
  10 #include <netdb.h>
  11 #include <string.h>
  12 #include <sys/time.h>
  13 #include <sys/types.h>
  14 #include <sys/socket.h>
  15 #include <netinet/in.h>
  16 #include <signal.h>
  17 #include <stdarg.h>
  18 #include <stdlib.h>
  19 #include <unistd.h>
  20 #include <time.h>
  21 #include "control.h"
  22
  23 int new_packet(short type, char *packet)
  24 {
  25         int id = (time(NULL) ^ (rand() * 1024*1024));
  26
  27         *(short *)(packet + 0) = ntohs(0x9012);
  28         *(short *)(packet + 2) = ntohs(type);
  29         *(int *)(packet + 6) = ntohl(id);
  30
  31         return 10;
  32 }
  33
  34 int send_packet(int sockfd, int dest_ip, int dest_port, char *packet, int len)
  35 {
  36         struct sockaddr_in addr;
  37
  38         *(short *)(packet + 4) = ntohs(len);
  39
  40         memset(&addr, 0, sizeof(addr));
  41         addr.sin_family = AF_INET;
  42         *(int*)&addr.sin_addr = htonl(dest_ip);
  43         addr.sin_port = htons(dest_port);
  44         if (sendto(sockfd, packet, len, 0, (void *) &addr, sizeof(addr)) < 0)
  45         {
  46                 perror("sendto");
  47                 return 0;
  48         }
  49         return 1;
  50 }
  51
  52 int read_packet(int sockfd, char *packet)
  53 {
  54         struct sockaddr_in addr;
  55         int alen = sizeof(addr);
  56         memset(&addr, 0, sizeof(addr));
  57         return recvfrom(sockfd, packet, 1400, 0, (void *) &addr, &alen);
  58 }
  59
  60 void dump_packet(char *packet, FILE *stream)
  61 {
  62         if (htons(*(short *)(packet + 0)) != 0x9012)
  63         {
  64                 fprintf(stream, "Invalid packet identifier %x\n", htons(*(short *)(packet + 0)));
  65                 return;
  66         }
  67         fprintf(stream, "Control packet:\n");
  68         fprintf(stream, "       Type: %d\n", htons(*(short *)(packet + 2)));
  69         fprintf(stream, "       Length: %d\n", htons(*(short *)(packet + 4)));
  70         fprintf(stream, "       Identifier: %x\n", htonl(*(int *)(packet + 6)));
  71         fprintf(stream, "\n");
  72 }
  73
  74