xt_GTPURH.c 20.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
/*
 * GTPu klm for Linux/iptables
 *
 * Copyright (c) 2010-2011 Polaris Networks
 * Author: Pradip Biswas <pradip_biswas@polarisnetworks.net>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/skbuff.h>
#include <linux/ip.h>
18
#include <linux/if_ether.h>
19 20
#include <linux/route.h> 
#include <net/checksum.h>
21
#include <net/ip.h>
22 23 24 25 26
#include <net/udp.h>
#include <net/inet_sock.h>
#include <net/route.h> 
#include <linux/netfilter/x_tables.h>
#include <linux/netfilter_ipv4/ip_tables.h>
gauthier's avatar
 
gauthier committed
27 28 29
#if defined(CONFIG_IP6_NF_IPTABLES) || defined(CONFIG_IP6_NF_IPTABLES_MODULE)
#    define WITH_IPV6 1
#endif
30 31 32 33
#include "xt_GTPURH.h"
#if !(defined KVERSION)
#error "Kernel version is not defined!!!! Exiting."
#endif
gauthier's avatar
 
gauthier committed
34
//-----------------------------------------------------------------------------
35 36 37
MODULE_LICENSE("GPL");
MODULE_AUTHOR("Pradip Biswas <pradip_biswas@polarisnetworks.net>");
MODULE_DESCRIPTION("GTPu Data Path extension on netfilter");
gauthier's avatar
 
gauthier committed
38
//-----------------------------------------------------------------------------
gauthier's avatar
 
gauthier committed
39 40 41 42 43 44 45 46
static char*         _gtpurh_nf_inet_hook_2_string(int nf_inet_hookP);
static inline bool   _gtpurh_ip_is_fragment(const struct iphdr *iph_p);
static void          _gtpurh_print_hex_octets(unsigned char* data_pP, unsigned short sizeP);
static unsigned int  _gtpurh_tg4_rem(struct sk_buff *orig_skb_pP, const struct xt_action_param *par_pP);
#if defined(WITH_IPV6)
static unsigned int  gtpurh_tg6(struct sk_buff *skb_pP, const struct xt_action_param *par_pP);
#endif
static unsigned int  gtpurh_tg4(struct sk_buff *skb_pP, const struct xt_action_param *par_pP);
gauthier's avatar
 
gauthier committed
47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65
//-----------------------------------------------------------------------------
//#define ROUTE_PACKET 1
#define NEW_SKB 1

#define GTPURH_HDR_PNBIT 1
#define GTPURH_HDR_SBIT 1 << 1
#define GTPURH_HDR_EBIT 1 << 2
#define GTPURH_ANY_EXT_HDR_BIT (GTPURH_HDR_PNBIT | GTPURH_HDR_SBIT | GTPURH_HDR_EBIT)
#define GTPURH_FAILURE 1
#define GTPURH_SUCCESS !GTPURH_FAILURE
#define GTPURH_PORT 2152
#define GTPURH_2_PRINT_BUFFER_LEN 8192
#define NIPADDR(addr) \
        (uint8_t)(addr & 0x000000FF), \
        (uint8_t)((addr & 0x0000FF00) >> 8), \
        (uint8_t)((addr & 0x00FF0000) >> 16), \
        (uint8_t)((addr & 0xFF000000) >> 24)
//-----------------------------------------------------------------------------
static char _gtpurh_print_buffer[GTPURH_2_PRINT_BUFFER_LEN];
66

gauthier's avatar
 
gauthier committed
67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89
static struct xt_target gtpurh_tg_reg[] __read_mostly = {
                 {
                                 .name           = "GTPURH",
                                 .revision       = 0,
                                 .family         = NFPROTO_IPV4,
                                 .hooks          = (1 << NF_INET_PRE_ROUTING) |
                                                   (1 << NF_INET_LOCAL_OUT),
                                 .proto          = IPPROTO_UDP,
                                 .table          = "raw",
                                 .target         = gtpurh_tg4,
                                 .targetsize     = sizeof(struct xt_gtpurh_target_info),
                                 .me             = THIS_MODULE,
                 },
#if defined(WITH_IPV6)
                 {
                                 .name       = "GTPURH",
                                 .revision   = 0,
                                 .family     = NFPROTO_IPV6,
                                 .proto      = IPPROTO_UDP,
                                 .table      = "raw",
                                 .target     = gtpurh_tg6,
                                 .me         = THIS_MODULE,
                },
gauthier's avatar
gauthier committed
90
#endif
gauthier's avatar
 
gauthier committed
91
};
gauthier's avatar
gauthier committed
92

93 94 95 96 97 98 99 100
struct gtpuhdr
{
    char flags;
    char msgtype;
    u_int16_t length;
    u_int32_t tunid;
};

gauthier's avatar
 
gauthier committed
101 102 103 104 105 106 107 108 109 110 111 112 113
//-----------------------------------------------------------------------------
static char*
_gtpurh_nf_inet_hook_2_string(int nf_inet_hookP) {
    //-----------------------------------------------------------------------------
    switch (nf_inet_hookP) {
        case NF_INET_PRE_ROUTING:   return "NF_INET_PRE_ROUTING";break;
        case NF_INET_LOCAL_IN:      return "NF_INET_LOCAL_IN";break;
        case NF_INET_FORWARD:       return "NF_INET_FORWARD";break;
        case NF_INET_LOCAL_OUT:     return "NF_INET_LOCAL_OUT";break;
        case NF_INET_POST_ROUTING:  return "NF_INET_POST_ROUTING";break;
        default: return "NF_INET_UNKNOWN";
    }
}
gauthier's avatar
gauthier committed
114

gauthier's avatar
 
gauthier committed
115 116 117 118
//-----------------------------------------------------------------------------
static inline bool
_gtpurh_ip_is_fragment(const struct iphdr *iph_p) {
    //-----------------------------------------------------------------------------
gauthier's avatar
 
gauthier committed
119 120
    return (iph_p->frag_off & htons(IP_MF | IP_OFFSET)) != 0;
}
gauthier's avatar
gauthier committed
121

122
//-----------------------------------------------------------------------------
gauthier's avatar
 
gauthier committed
123
void _gtpurh_print_hex_octets(unsigned char* data_pP, unsigned short sizeP)
124 125 126 127 128 129 130 131 132
//-----------------------------------------------------------------------------
{
  unsigned long octet_index = 0;
  unsigned long buffer_marker = 0;
  unsigned char aindex;
  struct timeval tv;
  char timeofday[64];
  unsigned int h,m,s;

gauthier's avatar
 
gauthier committed
133
  if (data_pP == NULL) {
134 135
    return;
  }
gauthier's avatar
 
gauthier committed
136 137 138
  if (sizeP > 2000) {
      return;
  }
139 140 141 142 143

  do_gettimeofday(&tv);
  h = (tv.tv_sec/3600) % 24;
  m = (tv.tv_sec / 60) % 60;
  s = tv.tv_sec % 60;
gauthier's avatar
gauthier committed
144
  snprintf(timeofday, 64, "%02d:%02d:%02d.%06ld", h,m,s,tv.tv_usec);
145 146 147 148

  buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
  buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker,"%s      |  0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f |\n",timeofday);
  buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
gauthier's avatar
 
gauthier committed
149
  pr_info("%s",_gtpurh_print_buffer);buffer_marker = 0;
150 151 152 153
  for (octet_index = 0; octet_index < sizeP; octet_index++) {
    if ((octet_index % 16) == 0){
      if (octet_index != 0) {
          buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
gauthier's avatar
 
gauthier committed
154
          pr_info("%s",_gtpurh_print_buffer);buffer_marker = 0;
155 156 157 158 159 160
      }
      buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, "%s %04ld |",timeofday, octet_index);
    }
    /*
     * Print every single octet in hexadecimal form
     */
gauthier's avatar
 
gauthier committed
161
    buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, " %02x", data_pP[octet_index]);
162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177
    /*
     * Align newline and pipes according to the octets in groups of 2
     */
  }

  /*
   * Append enough spaces and put final pipe
   */
  for (aindex = octet_index; aindex < 16; ++aindex)
    buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, "   ");
    //SGI_IF_DEBUG("   ");
  buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
  pr_info("%s",_gtpurh_print_buffer);
}


gauthier's avatar
 
gauthier committed
178
#if defined(ROUTE_PACKET)
gauthier's avatar
 
gauthier committed
179
//-----------------------------------------------------------------------------
gauthier's avatar
 
gauthier committed
180
static bool _gtpurh_route_packet(struct sk_buff *skb_pP, const struct xt_gtpurh_target_info *info_pP)
gauthier's avatar
 
gauthier committed
181
//-----------------------------------------------------------------------------
182
{
gauthier's avatar
 
gauthier committed
183 184 185 186 187
    int            err   = 0;
    struct rtable *rt    = NULL;
    struct iphdr  *iph_p = ip_hdr(skb_pP);
    int            daddr = iph_p->daddr;
    struct flowi   fl    = {
188 189 190
        .u = {
            .ip4 = {
                .daddr        = daddr,
gauthier's avatar
 
gauthier committed
191
                .flowi4_tos   = RT_TOS(iph_p->tos),
192 193 194 195
                .flowi4_scope = RT_SCOPE_UNIVERSE,
            } 
        } 
    }; 
196 197
    //skb_pP->pkt_type = PACKET_OTHERHOST;
    skb_pP->pkt_type = PACKET_OUTGOING;
gauthier's avatar
logs  
gauthier committed
198
#if 0
199
    pr_info("GTPURH(%d): Routing packet: %d.%d.%d.%d --> %d.%d.%d.%d Proto: %d, Len: %d Mark: %u Packet type: %u\n",
gauthier's avatar
 
gauthier committed
200 201 202 203 204 205 206 207 208 209 210
            info_pP->action,
            iph_p->saddr  & 0xFF,
            (iph_p->saddr & 0x0000FF00) >> 8,
            (iph_p->saddr & 0x00FF0000) >> 16,
            iph_p->saddr >> 24,
            iph_p->daddr  & 0xFF,
            (iph_p->daddr & 0x0000FF00) >> 8,
            (iph_p->daddr & 0x00FF0000) >> 16,
            iph_p->daddr >> 24,
            iph_p->protocol,
            ntohs(iph_p->tot_len),
211 212
            skb_pP->mark,
            skb_pP->pkt_type);
213 214
#endif
    rt = ip_route_output_key(&init_net, &fl.u.ip4);
gauthier's avatar
 
gauthier committed
215
    if (rt == null) {
gauthier's avatar
logs  
gauthier committed
216
        pr_info("GTPURH: Failed to route packet to dst 0x%x. Error: (%d)\n", fl.u.ip4.daddr, err);
217 218 219
        return GTPURH_FAILURE;
    } 

gauthier's avatar
logs  
gauthier committed
220
#if 0
gauthier's avatar
gauthier committed
221
    if (rt->dst.dev) {
222
        pr_info("GTPURH: dst dev name %s\n", rt->dst.dev->name);
gauthier's avatar
gauthier committed
223
    } else {
224 225 226 227
        pr_info("GTPURH: dst dev NULL\n");
    }
#endif

gauthier's avatar
gauthier committed
228 229 230 231
    skb_pP->priority = rt_tos2priority(iph_p->tos);
    skb_dst_drop(skb_pP);
    skb_dst_set(skb_pP, &rt->dst);
    skb_pP->dev      = skb_dst(skb_pP)->dev;
232

gauthier's avatar
 
gauthier committed
233
    // Send the GTPu message out
gauthier's avatar
gauthier committed
234
    ip_local_out(skb_pP);
235

gauthier's avatar
gauthier committed
236
    if (err == 0) {
237
        return GTPURH_SUCCESS;
gauthier's avatar
gauthier committed
238
    } else {
239 240 241
        return GTPURH_FAILURE;
    }
}
gauthier's avatar
 
gauthier committed
242
#endif
gauthier's avatar
gauthier committed
243

gauthier's avatar
 
gauthier committed
244
//-----------------------------------------------------------------------------
245
static unsigned int
gauthier's avatar
 
gauthier committed
246 247 248
_gtpurh_tg4_rem(struct sk_buff *orig_skb_pP, const struct xt_action_param *par_pP) {
//-----------------------------------------------------------------------------

gauthier's avatar
 
gauthier committed
249 250 251 252 253
    struct iphdr   *iph_p            = ip_hdr(orig_skb_pP);
    struct iphdr   *iph2_p           = NULL;
    struct udphdr  *udph_p           = NULL;
    struct gtpuhdr *gtpuh_p          = NULL;
    struct sk_buff *skb_p            = NULL;
gauthier's avatar
gauthier committed
254 255 256 257
#if defined(NEW_SKB)
    struct sk_buff *new_skb_p        = NULL;
    struct iphdr   *new_ip_p         = NULL;
#endif
258 259
    uint16_t        gtp_payload_size = 0;

gauthier's avatar
 
gauthier committed
260
    /* Create a new copy of the original skb_p...can't avoid :-( LG: WHY???*/
261
#if defined(ROUTE_PACKET)
gauthier's avatar
 
gauthier committed
262
    skb_p = skb_copy(orig_skb_pP, GFP_ATOMIC);
gauthier's avatar
gauthier committed
263
    if (skb_p == NULL) {
264 265
        return NF_ACCEPT;
    }
gauthier's avatar
 
gauthier committed
266
    skb_p->skb_iif  = orig_skb_pP->skb_iif;
gauthier's avatar
gauthier committed
267
    pr_info("GTPURH: skb protocol %04X\n", orig_skb_pP->protocol);
gauthier's avatar
 
gauthier committed
268
    skb_p->protocol = orig_skb_pP->protocol;
269
#else
gauthier's avatar
 
gauthier committed
270
    skb_p = orig_skb_pP;
gauthier's avatar
gauthier committed
271 272 273 274
    if (skb_linearize(skb_p) < 0) {
        pr_info("GTPURH: skb DROPPED (no linearize)\n");
        return NF_DROP;
    }
275
#endif
gauthier's avatar
 
gauthier committed
276 277 278
    //---------------------------
    // check if is GTPU TUNNEL
    if (iph_p->protocol != IPPROTO_UDP) {
gauthier's avatar
gauthier committed
279
        pr_info("GTPURH: skb DROPPED Not GTPV1U packet (not UDP)\n");
gauthier's avatar
 
gauthier committed
280
        return NF_ACCEPT;
gauthier's avatar
gauthier committed
281 282
    }

gauthier's avatar
 
gauthier committed
283 284 285 286 287
    //---------------------------
    // check if is fragment
    // but should not happen since MTU should have been set bigger than 1500 + GTP encap.
    // TO DO later segment, but did not succeed in getting in this target all framents of an ip packet!
    if (_gtpurh_ip_is_fragment(iph_p)) {
gauthier's avatar
gauthier committed
288
        pr_info("GTPURH: ip_is_fragment YES, FLAGS %04X & %04X = %04X\n",
gauthier's avatar
 
gauthier committed
289 290 291 292
                iph_p->frag_off,
                htons(IP_MF | IP_OFFSET),
                iph_p->frag_off & htons(IP_MF | IP_OFFSET));
            return NF_ACCEPT;
gauthier's avatar
gauthier committed
293
    }
gauthier's avatar
 
gauthier committed
294 295

    if (skb_p->len <= sizeof (struct udphdr) + sizeof (struct gtpuhdr) + sizeof (struct iphdr)) {
gauthier's avatar
gauthier committed
296
        pr_info("GTPURH: Thought was GTPV1U packet but too short length\n");
gauthier's avatar
 
gauthier committed
297 298
        return NF_ACCEPT;
    }
299
    /* Remove IP header */
gauthier's avatar
 
gauthier committed
300
    udph_p = (struct udphdr*)skb_pull(skb_p, (iph_p->ihl << 2));
301

gauthier's avatar
 
gauthier committed
302
    if (udph_p->dest != htons(GTPURH_PORT)) {
gauthier's avatar
 
gauthier committed
303
        pr_info("GTPURH: Not GTPV1U packet (bad UDP dest port)\n");
gauthier's avatar
 
gauthier committed
304
        skb_push(skb_p, (iph_p->ihl << 2));
gauthier's avatar
gauthier committed
305
        return NF_ACCEPT;
306
    }
307 308

    /* Remove UDP header */
gauthier's avatar
 
gauthier committed
309 310
    gtpuh_p = (struct gtpuhdr*)skb_pull(skb_p, sizeof(struct udphdr));
    gtp_payload_size = ntohs(gtpuh_p->length);
311

gauthier's avatar
 
gauthier committed
312
    skb_p->mark = ntohl(gtpuh_p->tunid);
313
    /* Remove GTPu header */
gauthier's avatar
 
gauthier committed
314
    skb_pull(skb_p, sizeof(struct gtpuhdr));
315 316

    /* If additional fields are present in header, remove them also */
gauthier's avatar
 
gauthier committed
317
    if (gtpuh_p->flags & GTPURH_ANY_EXT_HDR_BIT)
318
    {
gauthier's avatar
gauthier committed
319
        pr_info("GTPURH: GTPURH_ANY_EXT_HDR_BIT found\n");
gauthier's avatar
 
gauthier committed
320
        skb_pull(skb_p, sizeof(short) + sizeof(char) + sizeof(char)); /* #Seq, #N-PDU, #ExtHdr Type */
321 322
        gtp_payload_size = gtp_payload_size - sizeof(short) - sizeof(char) - sizeof(char);
    }
gauthier's avatar
 
gauthier committed
323 324 325
    skb_set_network_header(skb_p, 0);
    iph2_p   = ip_hdr(skb_p);
    skb_set_transport_header(skb_p, iph2_p->ihl << 2);
326

327

gauthier's avatar
 
gauthier committed
328
    if ((iph2_p->version  != 4 ) && (iph2_p->version  != 6)) {
gauthier's avatar
gauthier committed
329
        pr_info("\nGTPURH: Decapsulated packet dropped because not IPvx protocol see all GTPU packet here:\n");
gauthier's avatar
 
gauthier committed
330
        _gtpurh_print_hex_octets((unsigned char*)iph_p, ntohs(iph_p->tot_len));
gauthier's avatar
gauthier committed
331 332
        return NF_DROP;
    }
gauthier's avatar
logs  
gauthier committed
333
#if 0
gauthier's avatar
 
gauthier committed
334
    if ((skb_p->mark == 0) || (gtp_payload_size != ntohs(iph2_p->tot_len))) {
gauthier's avatar
 
gauthier committed
335
        pr_info("\nGTPURH: Decapsulated packet: %d.%d.%d.%d --> %d.%d.%d.%d Proto: %d, Total Len (IP): %u mark %u Frag offset %u Flags 0x%0x\n",
gauthier's avatar
 
gauthier committed
336 337 338 339 340 341 342 343 344 345 346 347 348 349 350
                iph2_p->saddr  & 0xFF,
                (iph2_p->saddr & 0x0000FF00) >> 8,
                (iph2_p->saddr & 0x00FF0000) >> 16,
                iph2_p->saddr >> 24,
                iph2_p->daddr  & 0xFF,
                (iph2_p->daddr & 0x0000FF00) >> 8,
                (iph2_p->daddr & 0x00FF0000) >> 16,
                iph2_p->daddr >> 24,
                iph2_p->protocol,
                ntohs(iph2_p->tot_len),
                skb_p->mark,
                ntohs(iph_p->frag_off) & 0x1FFFFFFF,
                ntohs(iph_p->frag_off) >> 13);

        if (gtp_payload_size != ntohs(iph2_p->tot_len)) {
gauthier's avatar
 
gauthier committed
351
            pr_info("GTPURH: Mismatch in lengths GTPU length: %u -> %u, IP length %u\n",
gauthier's avatar
 
gauthier committed
352
                    ntohs(gtpuh_p->length),
353
                    gtp_payload_size,
gauthier's avatar
 
gauthier committed
354
                    ntohs(iph2_p->tot_len));
355

gauthier's avatar
 
gauthier committed
356
            _gtpurh_print_hex_octets((unsigned char*)iph_p, ntohs(iph_p->tot_len));
gauthier's avatar
gauthier committed
357
            return NF_DROP;
358 359
        }
    }
gauthier's avatar
 
gauthier committed
360
#endif
361 362 363

    /* Route the packet */
#if defined(ROUTE_PACKET)
gauthier's avatar
 
gauthier committed
364
    _gtpurh_route_packet(skb_p, tgi_pP);
365 366
    return NF_DROP;
#else
367 368 369 370 371
    {
        int            err = 0;
        struct rtable *rt    = NULL;
        int            daddr = iph2_p->daddr;
        struct flowi   fl    = {
gauthier's avatar
gauthier committed
372 373 374 375 376 377 378 379
                           .u = {
                               .ip4 = {
                                   .daddr        = daddr,
                                   .flowi4_tos   = RT_TOS(iph2_p->tos),
                                   .flowi4_scope = RT_SCOPE_UNIVERSE,
                               }
                           }
                       };
380
        rt = ip_route_output_key(&init_net, &fl.u.ip4);
gauthier's avatar
gauthier committed
381
        if (rt == NULL) {
gauthier's avatar
logs  
gauthier committed
382
            pr_info("GTPURH: Failed to route packet to dst 0x%x. Error: (%d)\n", fl.u.ip4.daddr, err);
383 384
            return NF_DROP;
        }
gauthier's avatar
logs  
gauthier committed
385
#if 0
386 387 388 389
        if (rt->dst.dev) {
            pr_info("GTPURH: dst dev name %s\n", rt->dst.dev->name);
        } else {
            pr_info("GTPURH: dst dev NULL\n");
gauthier's avatar
 
gauthier committed
390
            return NF_DROP;
391 392
        }
#endif
gauthier's avatar
gauthier committed
393 394
        skb_p->priority = rt_tos2priority(iph2_p->tos);
        skb_p->pkt_type = PACKET_OTHERHOST;
395
        skb_dst_drop(skb_p);
gauthier's avatar
 
gauthier committed
396
        skb_dst_set(skb_p, dst_clone(&rt->dst));
397
        skb_p->dev      = skb_dst(skb_p)->dev;
gauthier's avatar
 
gauthier committed
398

gauthier's avatar
 
gauthier committed
399
#if defined(NEW_SKB)
gauthier's avatar
 
gauthier committed
400 401 402 403
        new_skb_p = alloc_skb(LL_MAX_HEADER + ntohs(iph2_p->tot_len), GFP_ATOMIC);
        if (new_skb_p == NULL) {
            return NF_DROP;
        }
gauthier's avatar
 
gauthier committed
404 405


gauthier's avatar
 
gauthier committed
406
        skb_reserve(new_skb_p, LL_MAX_HEADER + ntohs(iph2_p->tot_len));
gauthier's avatar
 
gauthier committed
407
        new_skb_p->protocol = skb_p->protocol;
gauthier's avatar
 
gauthier committed
408

gauthier's avatar
 
gauthier committed
409
        new_ip_p = (void *)skb_push(new_skb_p, ntohs(iph2_p->tot_len) - (iph2_p->ihl << 2));
gauthier's avatar
 
gauthier committed
410
        skb_reset_transport_header(new_skb_p);
gauthier's avatar
 
gauthier committed
411
        new_ip_p = (void *)skb_push(new_skb_p, iph2_p->ihl << 2);
gauthier's avatar
 
gauthier committed
412
        memcpy(new_ip_p, iph2_p, ntohs(iph2_p->tot_len));
gauthier's avatar
 
gauthier committed
413
        skb_reset_network_header(new_skb_p);
gauthier's avatar
 
gauthier committed
414 415
        skb_reset_inner_network_header(new_skb_p);
        skb_reset_inner_transport_header(new_skb_p);
gauthier's avatar
 
gauthier committed
416

gauthier's avatar
 
gauthier committed
417
        new_skb_p->mark = ntohl(gtpuh_p->tunid);
gauthier's avatar
 
gauthier committed
418 419
          //new_skb_p->mark     = skb_p->mark;

gauthier's avatar
 
gauthier committed
420 421
        /* ip_route_me_harder expects skb->dst to be set */
        skb_dst_set(new_skb_p, dst_clone(skb_dst(skb_p)));
gauthier's avatar
 
gauthier committed
422

gauthier's avatar
 
gauthier committed
423 424 425 426
        /*if (ip_route_me_harder(new_skb_p, RTN_UNSPEC) < 0) {
            pr_info("GTPURH: cannot route harder dest 0x%x\n", daddr);
            goto free_skb;
        }*/
gauthier's avatar
 
gauthier committed
427

gauthier's avatar
 
gauthier committed
428 429
        new_ip_p->ttl        = new_ip_p->ttl -1; // ip4_dst_hoplimit(skb_dst(new_skb_p));
        new_skb_p->ip_summed = CHECKSUM_NONE;
gauthier's avatar
 
gauthier committed
430

gauthier's avatar
 
gauthier committed
431 432 433 434
        if (new_skb_p->len > dst_mtu(skb_dst(new_skb_p))) {
            pr_info("GTPURH: bad length\n");
            goto free_skb;
        }
gauthier's avatar
 
gauthier committed
435

gauthier's avatar
 
gauthier committed
436
        nf_ct_attach(new_skb_p, skb_p);
gauthier's avatar
logs  
gauthier committed
437
        /*pr_info("GTPURH: ip_local_out %s/%s dev %s src %u.%u.%u.%u dst  %u.%u.%u.%u\n",
gauthier's avatar
 
gauthier committed
438 439 440 441
                _gtpurh_nf_inet_hook_2_string(par_pP->hooknum),
                gtpurh_tg_reg[0].table,
                (new_skb_p->dev == NULL) ? "NULL" : new_skb_p->dev->name,
                NIPADDR(new_ip_p->saddr),
gauthier's avatar
logs  
gauthier committed
442
                NIPADDR(new_ip_p->daddr));*/
gauthier's avatar
 
gauthier committed
443 444 445 446 447 448 449 450
        ip_local_out(new_skb_p);
        return NF_DROP;
free_skb:
        pr_info("GTPURH: Dropped skb ip_local_out %s/%s\n",
                _gtpurh_nf_inet_hook_2_string(par_pP->hooknum),
                gtpurh_tg_reg[0].table);
        kfree_skb(new_skb_p);
        return NF_DROP;
gauthier's avatar
 
gauthier committed
451 452

#else
gauthier's avatar
 
gauthier committed
453 454 455 456 457 458

        return NF_ACCEPT;
free_skb:
        pr_info("GTPURH: Dropped skb\n");
        kfree_skb(skb_p);
        return NF_DROP;
459
#endif
gauthier's avatar
 
gauthier committed
460
    }
gauthier's avatar
gauthier committed
461
#endif
462 463 464
}


gauthier's avatar
 
gauthier committed
465 466
#if defined(WITH_IPV6)
//-----------------------------------------------------------------------------
467
static unsigned int
gauthier's avatar
 
gauthier committed
468
gtpurh_tg6(struct sk_buff *skb_pP, const struct xt_action_param *par_pP)
gauthier's avatar
 
gauthier committed
469
//-----------------------------------------------------------------------------
470
{
gauthier's avatar
 
gauthier committed
471 472
    const struct xt_gtpurh_target_info *tgi_p = par_pP->targinfo;
    int result = NF_DROP;
473

gauthier's avatar
 
gauthier committed
474
    if (tgi_p == NULL) {
475 476 477
        return result;
    }

gauthier's avatar
 
gauthier committed
478 479
    if (tgi_p->action == PARAM_GTPURH_ACTION_REM) {
        result = NF_DROP; // TO DO
480 481 482
    }
    return result;
}
gauthier's avatar
 
gauthier committed
483
#endif
484

gauthier's avatar
 
gauthier committed
485
//-----------------------------------------------------------------------------
gauthier's avatar
 
gauthier committed
486 487
static unsigned int
gtpurh_tg4(struct sk_buff *skb_pP, const struct xt_action_param *par_pP)
gauthier's avatar
 
gauthier committed
488
//-----------------------------------------------------------------------------
489
{
gauthier's avatar
 
gauthier committed
490 491
    const struct iphdr                 *iph_p = ip_hdr(skb_pP);
    const struct rtable                *rt_p  = skb_rtable(skb_pP);
gauthier's avatar
 
gauthier committed
492 493 494
    const struct xt_gtpurh_target_info *tgi_p = par_pP->targinfo;

    if (tgi_p == NULL) {
gauthier's avatar
 
gauthier committed
495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515
        return NF_ACCEPT;
    }

    if ((tgi_p->raddr != iph_p->saddr) || (tgi_p->laddr != iph_p->daddr)) {
        pr_info("GTPURH: Not processed because of not matching saddr %d.%d.%d.%d raddr %d.%d.%d.%d  laddr %d.%d.%d.%d daddr %d.%d.%d.%d\n",
                NIPADDR(tgi_p->raddr),
                NIPADDR(iph_p->saddr),
                NIPADDR(tgi_p->laddr),
                NIPADDR(iph_p->daddr));
        return NF_ACCEPT;
    }

    if (ip_hdrlen(skb_pP) != sizeof(struct iphdr)) {
        pr_info("GTPURH: Dropped because IP options\n");
        return NF_DROP;
    }

    // Drop fragments
    if (iph_p->frag_off & htons(IP_OFFSET)) {
        pr_info("GTPURH: Dropped because is fragment\n");
        return NF_DROP;
gauthier's avatar
 
gauthier committed
516 517 518
    }

    if (tgi_p->action == PARAM_GTPURH_ACTION_REM) {
gauthier's avatar
 
gauthier committed
519
        return _gtpurh_tg4_rem(skb_pP, par_pP);
gauthier's avatar
 
gauthier committed
520
    }
gauthier's avatar
 
gauthier committed
521
    return NF_DROP;
gauthier's avatar
 
gauthier committed
522 523
}

524

gauthier's avatar
 
gauthier committed
525 526 527 528 529
//-----------------------------------------------------------------------------
static int
__init gtpurh_tg_init(void) {
//-----------------------------------------------------------------------------

530 531
    pr_info("GTPURH: Initializing module (KVersion: %d)\n", KVERSION);
    pr_info("GTPURH: Copyright Polaris Networks 2010-2011\n");
gauthier's avatar
 
gauthier committed
532
    pr_info("GTPURH: Modified by EURECOM Lionel GAUTHIER 2014\n");
533
#ifndef CMAKER
gauthier's avatar
gauthier committed
534
    pr_info("GTPURH: Compiled %s at time %s\n",__DATE__,__TIME__);
535
#endif
gauthier's avatar
 
gauthier committed
536 537 538 539 540
#if defined(WITH_IPV6)
    pr_info("GTPURH: IPv4/IPv6 enabled\n");
#else
    pr_info("GTPURH: IPv4 only enabled\n");
#endif
gauthier's avatar
 
gauthier committed
541
    return xt_register_targets(gtpurh_tg_reg, ARRAY_SIZE(gtpurh_tg_reg));
542 543
}

gauthier's avatar
 
gauthier committed
544 545 546 547 548
//-----------------------------------------------------------------------------
static void
__exit gtpurh_tg_exit(void) {
//-----------------------------------------------------------------------------

gauthier's avatar
 
gauthier committed
549
    xt_unregister_targets(gtpurh_tg_reg, ARRAY_SIZE(gtpurh_tg_reg));
550 551 552
    pr_info("GTPURH: Unloading module\n");
}

gauthier's avatar
 
gauthier committed
553 554 555 556
module_init(gtpurh_tg_init);
module_exit(gtpurh_tg_exit);
MODULE_ALIAS("ipt6_GTPURH");
MODULE_ALIAS("ipt_GTPURH");
557