hashsamp.c 8.99 KB
Newer Older
Arne Øslebø's avatar
Arne Øslebø committed
1
2
3
4
5
#include <stdlib.h>
#include <stdio.h>
#include <sys/shm.h>
#include <string.h>
#include <errno.h>
6
#include <pcap.h>		/* DLT_EN10MB */
's avatar
committed
7
#include <assert.h>
Arne Øslebø's avatar
Arne Øslebø committed
8
9
10
#include "mapidflib.h"
#include "mapidlib.h"
#include "mapidevices.h"
11
#include "mapi_errors.h"
Arne Øslebø's avatar
Arne Øslebø committed
12
13
14
15
16
17
#include "mapid.h"
#include "fhelp.h"
#include "hashsamp.h"
#include <sys/sem.h>
#include <netinet/in.h>
#include <asm/types.h>
's avatar
   
committed
18
#include <net/ethernet.h>      /* For ETHERTYPE_IP */
Arne Øslebø's avatar
Arne Øslebø committed
19
20
#include "debug.h"

21
22
#ifndef CHDLC_HDRLEN
#define CHDLC_HDRLEN		4
23
#endif
Arne Øslebø's avatar
Arne Øslebø committed
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

#define NUM_STATS 40

//Hashing based sampling

/* IP and TCP Packet code from linux kernel */
struct iphdr {
  //we use i386->little endian
  __u8    ihl:4,
    version:4;
  __u8    tos;
  __u16   tot_len;
  __u16   id;
  __u16   frag_off;
  __u8    ttl;
  __u8    protocol;
  __u16   check;
  __u32   saddr;
  __u32   daddr;
};

struct tcphdr {
  __u16   source;
  __u16   dest;
  __u32   seq;
  __u32   ack_seq;
  __u16   res1:4,
    doff:4,
    fin:1,
    syn:1,
    rst:1,
    psh:1,
    ack:1,
    urg:1,
    ece:1,
    cwr:1;
  __u16   window;
  __u16   check;
  __u16   urg_ptr;
};

struct udphdr {
        __u16   source;
        __u16   dest;
        __u16   len;
        __u16   check;
};



/* Hashing Code from the linux kernel*/

#define __jhash_mix(a, b, c) \
{ \
  a -= b; a -= c; a ^= (c>>13); \
  b -= c; b -= a; b ^= (a<<8); \
  c -= a; c -= b; c ^= (b>>13); \
  a -= b; a -= c; a ^= (c>>12);  \
  b -= c; b -= a; b ^= (a<<16); \
  c -= a; c -= b; c ^= (b>>5); \
  a -= b; a -= c; a ^= (c>>3);  \
  b -= c; b -= a; b ^= (a<<10); \
  c -= a; c -= b; c ^= (b>>15); \
}

/* The golden ration: an arbitrary value */
#define JHASH_GOLDEN_RATIO      0x9e3779b9


static inline int jhash_3words(int a, int b, int c, int initval)
{
        a += JHASH_GOLDEN_RATIO;
        b += JHASH_GOLDEN_RATIO;
        c += initval;

        __jhash_mix(a, b, c);

        return c;
}


static int hashsamp_instance(mapidflib_function_instance_t *instance,
106
107
			     MAPI_UNUSED int fd,
			     MAPI_UNUSED mapidflib_flow_mod_t *flow_mod)
Arne Øslebø's avatar
Arne Øslebø committed
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
{
  //check arguments
  mapiFunctArg* fargs;
  int samppart;
  int keepresults;
  
  fargs=instance->args;
  samppart=getargint(&fargs);
  keepresults=getargint(&fargs);

  if((samppart>1024)||(samppart<1))
    {
      return  MFUNCT_INVALID_ARGUMENT_1;
    }
  //keepresults is boolean
  if(keepresults!=0)
    {
      //we need shared memory
      int size;
      size=NUM_STATS*sizeof(struct sample)+sizeof(samp_buffer_t);
      instance->def->shm_size=size;
    }
  return 0;
}


static int hashsamp_init(mapidflib_function_instance_t *instance,
135
			 MAPI_UNUSED int fd)
Arne Øslebø's avatar
Arne Øslebø committed
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
{
  mapiFunctArg* fargs=instance->args;
  samp_buffer_t * sbuf;
  int samppart;
  int keepresults;
  samppart=getargint(&fargs);
  keepresults=getargint(&fargs);
  
  if(keepresults!=0)
    {
      samp_buffer_t* i;
      sbuf=instance->result.data;
      //we need a backup copy, to read keepresults
      i=instance->internal_data=malloc(sizeof(samp_buffer_t));
      i->keepresults=keepresults;
      sbuf->keepresults=keepresults;
    }
  else
    {
      sbuf=instance->internal_data=malloc(sizeof(samp_buffer_t));
      sbuf->keepresults=0;
    }
  sbuf->samppart=samppart;
  
  if(sbuf->keepresults!=0)
    {
      int ret;
      //create semaphore
      if((ret=fhlp_create_semaphore(&sbuf->sem,1))!=0) {
	DEBUG_CMD(printf("Error initializing semaphore: %d\n",ret));
	return ret;
      }
      sbuf->read_ptr=0;
      sbuf->write_ptr=0;
      sbuf->buf=(char*)sbuf+sizeof(samp_buffer_t);
      sbuf->cap_length=sizeof(struct sample);
      sbuf->bufsize=NUM_STATS*sizeof(struct sample);
      
    }
  return 0;
}

static int hashsamp_process(mapidflib_function_instance_t *instance,
's avatar
committed
179
180
		     MAPI_UNUSED unsigned char* dev_pkt,
		     unsigned char* pkt,
Arne Øslebø's avatar
Arne Øslebø committed
181
182
183
		     mapid_pkthdr_t* pkt_head)
{
  samp_buffer_t *mbuf=instance->internal_data;
's avatar
committed
184
  const char* data = NULL;
Arne Øslebø's avatar
Arne Øslebø committed
185
186
187
188
189
190
191
192
193
194
195

  struct iphdr *iphdr;
  struct tcphdr *tcphdr;
  struct udphdr *udphdr;
  unsigned int s;
  unsigned int d;
  unsigned int hash;

  if(mbuf->keepresults!=0)
    mbuf=instance->result.data;
  
196
197
198
199
200
201
202
203
204
  if (instance->hwinfo->link_type == DLT_EN10MB) /* ethernet */ {
      //pkt parsing...
      //first 14 bytes should be pkt_head...
      data=(const char*)pkt;
      if((data[12]!=8)||(data[13]!=0)) {
        //this is not an IP packet, (0x0800)
        return 0;
      }
      data=data+14; //ethernet header is 14bytes
's avatar
   
committed
205
  } else if (instance->hwinfo->link_type == DLT_CHDLC ) {
206
      data = (const char*)pkt;
207
208
209
210
      if ((u_int32_t)ntohl(*(const u_int32_t *)(&data[2])) != ETHERTYPE_IP) {
	//this is not an IP packet
	return 0;
      }
's avatar
   
committed
211
      data = data + CHDLC_HDRLEN;
's avatar
   
committed
212
213
214
  } else {
	  assert(0);
  }
215
  
Arne Øslebø's avatar
Arne Øslebø committed
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
  iphdr=(struct iphdr*)data;
  //we only sample TCP and UDP traffic
  //based on source IP&PORT, dest IP&PORT and the protocol
  if(iphdr->protocol!=6)
    {
      //TCP is IP protocol 6
      if(iphdr->protocol!=17)
	{
	  //UDP is IP protocol 17
	  return 0;
	}
    }
  
  data=data+4*iphdr->ihl;//ihl is in 32bit words...
  if (iphdr->protocol==6)
    {
      tcphdr=(struct tcphdr*)data;
      s=tcphdr->source;
      d=tcphdr->dest;
    }
  else
    {
      udphdr=(struct udphdr*)data;
      s=udphdr->source;
      d=udphdr->dest;
    }

  hash=((unsigned int)jhash_3words(iphdr->saddr, (iphdr->daddr ^ iphdr->protocol),(s | (d<< 16)),6564987))%1024;
#ifdef DEBUG  
  printf("hash: %d, threshold: %u\n",hash, mbuf->samppart);
#endif
  //we don't need any futher processing if no results should be kept.
  if(mbuf->keepresults==0)
    {
      if(hash<mbuf->samppart) return 1;
      return 0;
    }
  if(hash<mbuf->samppart)
    {
      //OK, hash succeeded
      struct sample* thesample=(struct sample*)malloc(sizeof(struct sample));
      
      unsigned new_write=mbuf->write_ptr+mbuf->cap_length;
      char* write;
      struct sembuf sem_add={0,1,IPC_NOWAIT};
      printf("Hash succeeded");
      thesample->source_ip=iphdr->saddr;
      thesample->dest_ip=iphdr->daddr;
      thesample->protocol=iphdr->protocol;
      thesample->sourceport=ntohs(s);
      thesample->destport=ntohs(d);
      thesample->timestamp=pkt_head->ts;
      if(thesample->protocol==6)
      {
	tcphdr=(struct tcphdr*)data;
	thesample->sequence=ntohl(tcphdr->seq);
	thesample->tcp_flags=0;
	if(tcphdr->fin==1) thesample->tcp_flags+=TCP_FIN;
	if(tcphdr->syn==1) thesample->tcp_flags+=TCP_SYN;
	if(tcphdr->rst==1) thesample->tcp_flags+=TCP_RST;
	if(tcphdr->ack==1) thesample->tcp_flags+=TCP_ACK;
	if(tcphdr->psh==1) thesample->tcp_flags+=TCP_PSH;
      }
      
      if(new_write+mbuf->cap_length>mbuf->bufsize)
	//Check to see if there is room at the end of the buffer for a new packet
	//If not, wrap to beginning of buffer
	new_write=0;
      
      if(new_write==mbuf->read_ptr
	 || (new_write<mbuf->write_ptr && new_write>mbuf->read_ptr) 
	 || (mbuf->write_ptr<mbuf->read_ptr && new_write>mbuf->read_ptr && new_write>mbuf->write_ptr))
	//Check to see if buffer is full
#ifdef DEBUG
	printf("Packet dropped\n");
#endif
      else {
	//Enough space in buffer for the new packet
	write=mbuf->buf+mbuf->write_ptr;
	memcpy(write,thesample,sizeof(struct sample));
	
	if(semop(mbuf->sem.id,&sem_add,1)==-1)
	  {
	    //error....
	    printf("Error\n");
	    /* should be handled in some way */
	  }
	mbuf->write_ptr=new_write;
	mbuf->count1++;
      }
      free(thesample);	
      return 1;
    }
  return 0;
}


static int hashsamp_cleanup(mapidflib_function_instance_t *instance) 
{
  samp_buffer_t *mbuf;
  mbuf=instance->internal_data;
  if(mbuf->keepresults!=0)
  {
    mbuf=instance->result.data;
    
    fhlp_free_semaphore(&mbuf->sem);
  }

  free(instance->internal_data);
 
  return 0;
}



static int hashsamp_client_read_result(mapidflib_function_instance_t *instance,mapi_result_t *res) 
{
  samp_buffer_t *tb=instance->result.data;
  char *buf=(char*)instance->result.data+sizeof(samp_buffer_t);
  struct sembuf sem_sub={0,-1,0};  
  union semun {
		int val;
		struct semid_ds *buf;
		ushort * array;
	} argument;

  int condid;

  argument.val = 1;  
  //wait for packet(semaphore blocks when no packets ware ready in the buffer)
  condid=semget(tb->sem.key,1,IPC_CREAT|0660);
  //
  if((errno=semop(condid,&sem_sub,1))==-1)
    {
      //error....
#ifdef DEBUG
      printf("Error in semop\n"); //Debug
#endif
      return MAPI_SEM_ERR;
    }
  
  //Wait for new pkt

  res->res=buf+tb->read_ptr;
  tb->read_ptr=tb->read_ptr+tb->cap_length;
  if(tb->read_ptr+tb->cap_length>tb->bufsize)
    tb->read_ptr=0;

  res->size=sizeof(struct sample);

  //semop(condid,&sem_wait_sub,1);
  return 0;
}


static mapidflib_function_def_t finfo={
  "", //libname
  "HASHSAMP", //name
  "Implements hashing based sampling",  //descr
  "ii", //argdescr
  MAPI_DEVICE_ALL, //devtype
  MAPIRES_SHM,
  0, //shm size. Set by instance
  0, //modifies_pkts
380
381
  1, //filters packets
  MAPIOPT_AUTO, //Optimization
Arne Øslebø's avatar
Arne Øslebø committed
382
383
384
385
386
387
  hashsamp_instance,
  hashsamp_init,
  hashsamp_process,
  NULL, //get_result
  NULL, //reset
  hashsamp_cleanup,
388
  NULL, //client_init
Arne Øslebø's avatar
Arne Øslebø committed
389
390
391
392
393
394
395
396
  hashsamp_client_read_result,
  NULL //client_cleanup
};

mapidflib_function_def_t* hashsamp_get_funct_info();
mapidflib_function_def_t* hashsamp_get_funct_info() {
  return &finfo;
};