common.c 17.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 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
/*******************************************************************************

  Eurecom OpenAirInterface 2
  Copyright(c) 1999 - 2010 Eurecom

  This program is free software; you can redistribute it and/or modify it
  under the terms and conditions of the GNU General Public License,
  version 2, as published by the Free Software Foundation.

  This program is distributed in the hope it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  more details.

  You should have received a copy of the GNU General Public License along with
  this program; if not, write to the Free Software Foundation, Inc.,
  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.

  The full GNU General Public License is included in this distribution in
  the file called "COPYING".

  Contact Information
  Openair Admin: openair_admin@eurecom.fr
  Openair Tech : openair_tech@eurecom.fr
  Forums       : http://forums.eurecom.fsr/openairinterface
  Address      : Eurecom, 2229, route des crêtes, 06560 Valbonne Sophia Antipolis, France

*******************************************************************************/

/*! \file common.c
* \brief implementation of emultor tx and rx
* \author Navid Nikaein and Raymomd Knopp, Lionel GAUTHIER
* \date 2011
* \version 1.0
* \company Eurecom
* \email: navid.nikaein@eurecom.fr, lionel.gauthier@eurecom.fr
*/

#include "local.h"
#include "proto_extern.h"
#ifndef OAI_NW_DRIVER_USE_NETLINK
#include "rtai_fifos.h"
#endif


#include <linux/inetdevice.h>
#ifdef OAI_NW_DRIVER_TYPE_ETHERNET
#include <linux/etherdevice.h>
#endif

#include <net/tcp.h>
#include <net/udp.h>

#define NIPADDR(addr) \
        (uint8_t)(addr & 0x000000FF), \
        (uint8_t)((addr & 0x0000FF00) >> 8), \
        (uint8_t)((addr & 0x00FF0000) >> 16), \
        (uint8_t)((addr & 0xFF000000) >> 24)

#define NIP6ADDR(addr) \
        ntohs((addr)->s6_addr16[0]), \
        ntohs((addr)->s6_addr16[1]), \
        ntohs((addr)->s6_addr16[2]), \
        ntohs((addr)->s6_addr16[3]), \
        ntohs((addr)->s6_addr16[4]), \
        ntohs((addr)->s6_addr16[5]), \
        ntohs((addr)->s6_addr16[6]), \
        ntohs((addr)->s6_addr16[7])


71 72
//#define OAI_DRV_DEBUG_SEND
//#define OAI_DRV_DEBUG_RECEIVE
73
void oai_nw_drv_common_class_wireless2ip(uint16_t dlen,
74 75 76 77 78 79 80 81
                        void *pdcp_sdu,
                        int inst,
                        OaiNwDrvRadioBearerId_t rb_id) {

  //---------------------------------------------------------------------------
    struct sk_buff      *skb;
    struct ipversion    *ipv;
    struct oai_nw_drv_priv     *gpriv=netdev_priv(oai_nw_drv_dev[inst]);
82
    unsigned int         hard_header_len = 0;
83 84
    uint16_t                 *p_ether_type;
    uint16_t                  ether_type;
85 86 87 88 89 90 91 92 93 94
    #ifdef OAI_DRV_DEBUG_RECEIVE
    int i;
    unsigned char *addr;
    #endif
    unsigned char        protocol;
    struct iphdr        *network_header;

    #ifdef OAI_DRV_DEBUG_RECEIVE
    printk("[OAI_IP_DRV][%s] begin RB %d Inst %d Length %d bytes\n",__FUNCTION__, rb_id,inst,dlen);
    #endif
95

96 97 98 99 100 101 102 103 104 105 106 107 108
    skb = dev_alloc_skb( dlen + 2 );

    if(!skb) {
        printk("[OAI_IP_DRV][%s] low on memory\n",__FUNCTION__);
        ++gpriv->stats.rx_dropped;
        return;
    }
    skb_reserve(skb,2);
    memcpy(skb_put(skb, dlen), pdcp_sdu,dlen);

    skb->dev = oai_nw_drv_dev[inst];
    hard_header_len = oai_nw_drv_dev[inst]->hard_header_len;

109 110 111
    skb_set_mac_header(skb, 0);
    skb_set_network_header(skb, hard_header_len);
    skb->mark = rb_id;
112 113 114 115
    skb->pkt_type = PACKET_HOST;


    #ifdef OAI_DRV_DEBUG_RECEIVE
116
    printk("[OAI_IP_DRV][%s] Receiving packet @%p of size %d from PDCP \n",__FUNCTION__, skb->data, skb->len);
117 118 119 120 121 122 123 124

    for (i=0;i<skb->len;i++)
        printk("%2x ",((unsigned char *)(skb->data))[i]);
    printk("\n");
    #endif



125 126
    // LG TEST skb->ip_summed = CHECKSUM_NONE;
    skb->ip_summed = CHECKSUM_UNNECESSARY;
127 128


129
    ipv = (struct ipversion *)skb_network_header(skb);
130

131 132 133 134 135 136 137 138 139 140 141 142 143
    switch (ipv->version) {

        case 6:
            #ifdef OAI_DRV_DEBUG_RECEIVE
            printk("[OAI_IP_DRV][%s] receive IPv6 message\n",__FUNCTION__);
            #endif
            skb_set_network_header(skb, hard_header_len);
            //skb->network_header = &skb->data[hard_header_len];
            if (hard_header_len == 0) {
                skb->protocol = htons(ETH_P_IPV6);
            } else {
                #ifdef OAI_NW_DRIVER_TYPE_ETHERNET
                skb->protocol = eth_type_trans(skb, oai_nw_drv_dev[inst]);
144 145
                #else
                #endif
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
            }
            //printk("Writing packet with protocol %x\n",ntohs(skb->protocol));
            break;

        case 4:
            #ifdef NAS_ADDRESS_FIX
            // Make the third byte of both the source and destination equal to the fourth of the destination
            daddr = (unsigned char *)&((struct iphdr *)&skb->data[hard_header_len])->daddr;
            odaddr = ((struct iphdr *)skb->data)->daddr;
            //sn = addr[3];
            saddr = (unsigned char *)&((struct iphdr *)&skb->data[hard_header_len])->saddr;
            osaddr = ((struct iphdr *)&skb->data[hard_header_len])->saddr;

            if (daddr[0] == saddr[0]) {// same network
                daddr[2] = daddr[3]; // set third byte of destination to that of local machine so that local IP stack accepts the packet
                saddr[2] = daddr[3]; // set third byte of source to that of local machine so that local IP stack accepts the packet
            }  else { // get the 3rd byte from device address in net_device structure
                ifaddr = (unsigned char *)(&(((struct in_device *)((oai_nw_drv_dev[inst])->ip_ptr))->ifa_list->ifa_local));
                if (saddr[0] == ifaddr[0]) { // source is in same network as local machine
                    daddr[0] += saddr[3];        // fix address of remote destination to undo change at source
                    saddr[2] =  ifaddr[2];       // set third byte to that of local machine so that local IP stack accepts the packet
                } else {                         // source is remote machine from outside network
                    saddr[0] -= daddr[3];        // fix address of remote source to be understood by destination
                    daddr[2] =  daddr[3];        // fix 3rd byte of local address to be understood by IP stack of
                    // destination
171
                }
172 173 174 175 176 177
            }
            #endif //NAS_ADDRESS_FIX
            #ifdef OAI_DRV_DEBUG_RECEIVE
            //printk("NAS_TOOL_RECEIVE: receive IPv4 message\n");
            addr = (unsigned char *)&((struct iphdr *)&skb->data[hard_header_len])->saddr;
            if (addr) {
178 179
                //addr[2]^=0x01;
                printk("[OAI_IP_DRV][%s] Source %d.%d.%d.%d\n",__FUNCTION__, addr[0],addr[1],addr[2],addr[3]);
180 181 182
            }
            addr = (unsigned char *)&((struct iphdr *)&skb->data[hard_header_len])->daddr;
            if (addr){
183 184
                //addr[2]^=0x01;
                printk("[OAI_IP_DRV][%s] Dest %d.%d.%d.%d\n",__FUNCTION__, addr[0],addr[1],addr[2],addr[3]);
185 186 187
            }
            printk("[OAI_IP_DRV][%s] protocol  %d\n",__FUNCTION__, ((struct iphdr *)&skb->data[hard_header_len])->protocol);
            #endif
188

189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214
            skb_set_network_header(skb, hard_header_len);
            //skb->network_header = &skb->data[hard_header_len];
            network_header = (struct iphdr *)skb_network_header(skb);
            protocol = network_header->protocol;

            #ifdef OAI_DRV_DEBUG_RECEIVE
            switch (protocol) {
                case IPPROTO_IP:
                    printk("[OAI_IP_DRV][%s] Received Raw IPv4 packet\n",__FUNCTION__);
                    break;
                case IPPROTO_IPV6:
                    printk("[OAI_IP_DRV][%s] Received Raw IPv6 packet\n",__FUNCTION__);
                    break;
                case IPPROTO_ICMP:
                    printk("[OAI_IP_DRV][%s] Received Raw ICMP packet\n",__FUNCTION__);
                    break;
                case IPPROTO_TCP:
                    printk("[OAI_IP_DRV][%s] Received TCP packet\n",__FUNCTION__);
                    break;
                case IPPROTO_UDP:
                    printk("[OAI_IP_DRV][%s] Received UDP packet\n",__FUNCTION__);
                    break;
                default:
                    break;
            }
            #endif
215

216 217 218 219 220 221 222 223
            #ifdef NAS_ADDRESS_FIX
                network_header->check = 0;
                network_header->check = ip_fast_csum((unsigned char *) network_header, network_header->ihl);
                //printk("[OAI_IP_DRV][COMMON][RECEIVE] IP Fast Checksum %x \n", network_header->check);

            switch(protocol) {
                case IPPROTO_TCP:

224
                    cksum  = (uint16_t*)&(((struct tcphdr*)((network_header + (network_header->ihl<<2))))->check);
225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243
                    check  = csum_tcpudp_magic(((struct iphdr *)network_header)->saddr, ((struct iphdr *)network_header)->daddr, 0,0, ~(*cksum));
                    //check  = csum_tcpudp_magic(((struct iphdr *)network_header)->saddr, ((struct iphdr *)network_header)->daddr, tcp_hdrlen(skb), IPPROTO_TCP, ~(*cksum));
                    //check  = csum_tcpudp_magic(((struct iphdr *)network_header)->saddr, ((struct iphdr *)network_header)->daddr, dlen, IPPROTO_TCP, ~(*cksum));

                    *cksum = csum_tcpudp_magic(~osaddr, ~odaddr, 0, 0, ~check);
                    //*cksum = csum_tcpudp_magic(~osaddr, ~odaddr, dlen, IPPROTO_TCP, ~check);
                    #ifdef OAI_DRV_DEBUG_RECEIVE
                    printk("[OAI_IP_DRV][%s] Inst %d TCP packet calculated CS %x, CS = %x (before), SA (%x)%x, DA (%x)%x\n",__FUNCTION__,
                            inst,
                            network_header->check,
                            *cksum,
                            osaddr,
                            ((struct iphdr *)skb->data)->saddr,
                            odaddr,
                            ((struct iphdr *)skb->data)->daddr);

                    printk("[OAI_IP_DRV][%s] Inst %d TCP packet NEW CS %x\n",__FUNCTION__,
                            inst,
                            *cksum);
244
                    #endif
245
                    break;
246

247
                case IPPROTO_UDP:
248
                    cksum  = (uint16_t*)&(((struct udphdr*)((network_header + (network_header->ihl<<2))))->check);
249 250 251
                    check = csum_tcpudp_magic(((struct iphdr *)network_header)->saddr, ((struct iphdr *)network_header)->daddr, 0,0, ~(*cksum));
                    // check = csum_tcpudp_magic(((struct iphdr *)network_header)->saddr, ((struct iphdr *)network_header)->daddr, udp_hdr(skb)->len, IPPROTO_UDP, ~(*cksum));
                    //check = csum_tcpudp_magic(((struct iphdr *)network_header)->saddr, ((struct iphdr *)network_header)->daddr, dlen, IPPROTO_UDP, ~(*cksum));
252

253 254 255
                    *cksum= csum_tcpudp_magic(~osaddr, ~odaddr,0,0, ~check);
                    //*cksum= csum_tcpudp_magic(~osaddr, ~odaddr,udp_hdr(skb)->len, IPPROTO_UDP, ~check);
                    //*cksum= csum_tcpudp_magic(~osaddr, ~odaddr,dlen, IPPROTO_UDP, ~check);
256

257 258 259
                    #ifdef OAI_DRV_DEBUG_RECEIVE
                    printk("[OAI_IP_DRV][%s] Inst %d UDP packet CS = %x (before), SA (%x)%x, DA (%x)%x\n",__FUNCTION__,
                        inst,*cksum,osaddr,((struct iphdr *)&skb->data[hard_header_len])->saddr,odaddr,((struct iphdr *)&skb->data[hard_header_len])->daddr);
260

261
                    printk("[OAI_IP_DRV][%s] Inst %d UDP packet NEW CS %x\n",__FUNCTION__,inst,*cksum);
262
                    #endif
263 264 265 266
                    //if ((check = *cksum) != 0) {
                    // src, dst, len, proto, sum
                    //          }
                    break;
267

268 269 270 271 272 273 274
                default:
                    break;
            }
            #endif //NAS_ADDRESS_FIX
            if (hard_header_len == 0) {
                skb->protocol = htons(ETH_P_IP);
            } else {
275
                #ifdef OAI_NW_DRIVER_TYPE_ETHERNET
276 277
                skb->protocol = eth_type_trans(skb, oai_nw_drv_dev[inst]);
                #else
278
                #endif
279 280 281 282 283 284 285
            }
            //printk("[OAI_IP_DRV][COMMON] Writing packet with protocol %x\n",ntohs(skb->protocol));
            break;

        default:
                // fill skb->pkt_type, skb->dev

286
                skb->protocol = eth_type_trans(skb, oai_nw_drv_dev[inst]);
287 288
                // minus 1(short) instead of 2(bytes) because uint16_t*
                p_ether_type = (uint16_t *)(skb_network_header(skb)-2);
289
                ether_type = ntohs(*p_ether_type);
290

291 292 293 294 295 296 297 298 299 300 301
                switch (ether_type) {
                    case ETH_P_ARP:
                        #ifdef OAI_DRV_DEBUG_RECEIVE
                        printk("[OAI_IP_DRV][%s] ether_type = ETH_P_ARP\n",__FUNCTION__);
                        #endif
                        //skb->pkt_type = PACKET_HOST;
                        skb->protocol = htons(ETH_P_ARP);
                        break;
                    default:
                        ;
                }
302 303
            printk("[OAI_IP_DRV][%s] begin RB %d Inst %d Length %d bytes\n",__FUNCTION__,rb_id,inst,dlen);
            printk("[OAI_IP_DRV][%s] Inst %d: receive unknown message (version=%d)\n",__FUNCTION__,inst,ipv->version);
304
    }
305

306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321
    ++gpriv->stats.rx_packets;
    gpriv->stats.rx_bytes += dlen;
    #ifdef OAI_DRV_DEBUG_RECEIVE
    printk("[OAI_IP_DRV][%s] sending packet of size %d to kernel\n",__FUNCTION__,skb->len);
    for (i=0;i<skb->len;i++)
        printk("%2x ",((unsigned char *)(skb->data))[i]);
    printk("\n");
    #endif //OAI_DRV_DEBUG_RECEIVE
    netif_rx(skb);
    #ifdef OAI_DRV_DEBUG_RECEIVE
    printk("[OAI_IP_DRV][%s] end\n",__FUNCTION__);
    #endif
}

//---------------------------------------------------------------------------
// Delete the data
322
void oai_nw_drv_common_ip2wireless_drop(struct sk_buff *skb, int inst){
323 324 325 326 327 328 329
  //---------------------------------------------------------------------------
  struct oai_nw_drv_priv *priv=netdev_priv(oai_nw_drv_dev[inst]);
  ++priv->stats.tx_dropped;
}

//---------------------------------------------------------------------------
// Request the transfer of data (QoS SAP)
330
void oai_nw_drv_common_ip2wireless(struct sk_buff *skb, int inst){
331
  //---------------------------------------------------------------------------
332
  struct pdcp_data_req_header_s     pdcph;
333 334 335 336 337 338 339 340 341 342 343 344
  struct oai_nw_drv_priv *priv=netdev_priv(oai_nw_drv_dev[inst]);
#ifdef LOOPBACK_TEST
  int i;
#endif
#ifdef OAI_DRV_DEBUG_SEND
  int j;
#endif
  unsigned int bytes_wrote;
  // Start debug information
#ifdef OAI_DRV_DEBUG_SEND
  printk("[OAI_IP_DRV][%s] inst %d begin \n",__FUNCTION__,inst);
#endif
345

346 347 348 349 350 351
  if (skb==NULL){
#ifdef OAI_DRV_DEBUG_SEND
    printk("[OAI_IP_DRV][%s] input parameter skb is NULL \n",__FUNCTION__);
#endif
    return;
  }
352

353 354 355 356 357
  pdcph.data_size    = skb->len;
  pdcph.rb_id        = skb->mark;
  pdcph.inst         = inst;
  pdcph.traffic_type = oai_nw_drv_find_traffic_type(skb);

358 359 360

  bytes_wrote = oai_nw_drv_netlink_send((char *)&pdcph,OAI_NW_DRV_PDCPH_SIZE);
#ifdef OAI_DRV_DEBUG_SEND
Lionel Gauthier's avatar
Lionel Gauthier committed
361 362
  printk("[OAI_IP_DRV][%s] Wrote %d bytes (header for %d byte skb) to PDCP RB %d via netlink\n",__FUNCTION__,
  	       bytes_wrote,skb->len, pdcph.rb_id);
363
#endif
364

365 366 367 368

  if (bytes_wrote != OAI_NW_DRV_PDCPH_SIZE)
    {
      printk("[OAI_IP_DRV][%s] problem while writing PDCP's header (bytes wrote = %d to fifo %d)\n",__FUNCTION__,bytes_wrote,IP2PDCP_FIFO);
369
      printk("rb_id %u, Wrote %u, Header Size %lu \n", pdcph.rb_id , bytes_wrote, OAI_NW_DRV_PDCPH_SIZE);
370 371 372 373 374
      priv->stats.tx_dropped ++;
      return;
    }

  bytes_wrote += oai_nw_drv_netlink_send((char *)skb->data,skb->len);
375

376 377 378

  if (bytes_wrote != skb->len+OAI_NW_DRV_PDCPH_SIZE)
    {
379
      printk("[OAI_IP_DRV][%s] Inst %d, RB_ID %u: problem while writing PDCP's data, bytes_wrote = %u, Data_len %u, PDCPH_SIZE %lu\n",
380 381 382 383 384 385
             __FUNCTION__,
	     inst,
             pdcph.rb_id,
             bytes_wrote,
             skb->len,
             OAI_NW_DRV_PDCPH_SIZE); // congestion
386

387 388 389 390
      priv->stats.tx_dropped ++;
      return;
    }
#ifdef OAI_DRV_DEBUG_SEND
391
  printk("[OAI_IP_DRV][%s] Sending packet of size %d to PDCP traffic type %d\n",__FUNCTION__,skb->len, pdcph.traffic_type);
392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409

 for (j=0;j<skb->len;j++)
    printk("%2x ",((unsigned char *)(skb->data))[j]);
  printk("\n");
#endif

  priv->stats.tx_bytes   += skb->len;
  priv->stats.tx_packets ++;
#ifdef OAI_DRV_DEBUG_SEND
  printk("[OAI_IP_DRV][%s] end \n",__FUNCTION__);
#endif
}


//---------------------------------------------------------------------------
void oai_nw_drv_common_wireless2ip(struct nlmsghdr *nlh) {
//---------------------------------------------------------------------------

410
  struct pdcp_data_ind_header_s     *pdcph = (struct pdcp_data_ind_header_s *)NLMSG_DATA(nlh);
411 412 413 414 415 416 417 418 419
  struct oai_nw_drv_priv *priv;

  priv = netdev_priv(oai_nw_drv_dev[pdcph->inst]);

#ifdef OAI_DRV_DEBUG_RECEIVE
  printk("[OAI_IP_DRV][%s] QOS receive from PDCP, size %d, rab %d, inst %d\n",__FUNCTION__,
         pdcph->data_size,pdcph->rb_id,pdcph->inst);
#endif //OAI_DRV_DEBUG_RECEIVE

420
  oai_nw_drv_common_class_wireless2ip(pdcph->data_size,
421 422 423 424 425 426
                       (unsigned char *)NLMSG_DATA(nlh) + OAI_NW_DRV_PDCPH_SIZE,
                       pdcph->inst,
                       pdcph->rb_id);
}