SAFE public projects git trees. - safe/jmp/linux-2.6/blob - net/phonet/pep-gprs.c

   1 /*
   2  * File: pep-gprs.c
   3  *
   4  * GPRS over Phonet pipe end point socket
   5  *
   6  * Copyright (C) 2008 Nokia Corporation.
   7  *
   8  * Author: Rémi Denis-Courmont <remi.denis-courmont@nokia.com>
   9  *
  10  * This program is free software; you can redistribute it and/or
  11  * modify it under the terms of the GNU General Public License
  12  * version 2 as published by the Free Software Foundation.
  13  *
  14  * This program is distributed in the hope that it will be useful, but
  15  * WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  * General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public License
  20  * along with this program; if not, write to the Free Software
  21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  22  * 02110-1301 USA
  23  */
  24
  25 #include <linux/kernel.h>
  26 #include <linux/netdevice.h>
  27 #include <linux/if_ether.h>
  28 #include <linux/if_arp.h>
  29 #include <net/sock.h>
  30
  31 #include <linux/if_phonet.h>
  32 #include <net/tcp_states.h>
  33 #include <net/phonet/gprs.h>
  34
  35 #define GPRS_DEFAULT_MTU 1400
  36
  37 struct gprs_dev {
  38         struct sock             *sk;
  39         void                    (*old_state_change)(struct sock *);
  40         void                    (*old_data_ready)(struct sock *, int);
  41         void                    (*old_write_space)(struct sock *);
  42
  43         struct net_device       *dev;
  44
  45         struct sk_buff_head     tx_queue;
  46         struct work_struct      tx_work;
  47         spinlock_t              tx_lock;
  48         unsigned                tx_max;
  49 };
  50
  51 static __be16 gprs_type_trans(struct sk_buff *skb)
  52 {
  53         const u8 *pvfc;
  54         u8 buf;
  55
  56         pvfc = skb_header_pointer(skb, 0, 1, &buf);
  57         if (!pvfc)
  58                 return htons(0);
  59         /* Look at IP version field */
  60         switch (*pvfc >> 4) {
  61         case 4:
  62                 return htons(ETH_P_IP);
  63         case 6:
  64                 return htons(ETH_P_IPV6);
  65         }
  66         return htons(0);
  67 }
  68
  69 /*
  70  * Socket callbacks
  71  */
  72
  73 static void gprs_state_change(struct sock *sk)
  74 {
  75         struct gprs_dev *gp = sk->sk_user_data;
  76
  77         if (sk->sk_state == TCP_CLOSE_WAIT) {
  78                 struct net_device *dev = gp->dev;
  79
  80                 netif_stop_queue(dev);
  81                 netif_carrier_off(dev);
  82         }
  83 }
  84
  85 static int gprs_recv(struct gprs_dev *gp, struct sk_buff *skb)
  86 {
  87         struct net_device *dev = gp->dev;
  88         int err = 0;
  89         __be16 protocol = gprs_type_trans(skb);
  90
  91         if (!protocol) {
  92                 err = -EINVAL;
  93                 goto drop;
  94         }
  95
  96         if (likely(skb_headroom(skb) & 3)) {
  97                 struct sk_buff *rskb, *fs;
  98                 int flen = 0;
  99
 100                 /* Phonet Pipe data header is misaligned (3 bytes),
 101                  * so wrap the IP packet as a single fragment of an head-less
 102                  * socket buffer. The network stack will pull what it needs,
 103                  * but at least, the whole IP payload is not memcpy'd. */
 104                 rskb = netdev_alloc_skb(dev, 0);
 105                 if (!rskb) {
 106                         err = -ENOBUFS;
 107                         goto drop;
 108                 }
 109                 skb_shinfo(rskb)->frag_list = skb;
 110                 rskb->len += skb->len;
 111                 rskb->data_len += rskb->len;
 112                 rskb->truesize += rskb->len;
 113
 114                 /* Avoid nested fragments */
 115                 for (fs = skb_shinfo(skb)->frag_list; fs; fs = fs->next)
 116                         flen += fs->len;
 117                 skb->next = skb_shinfo(skb)->frag_list;
 118                 skb_shinfo(skb)->frag_list = NULL;
 119                 skb->len -= flen;
 120                 skb->data_len -= flen;
 121                 skb->truesize -= flen;
 122
 123                 skb = rskb;
 124         }
 125
 126         skb->protocol = protocol;
 127         skb_reset_mac_header(skb);
 128         skb->dev = dev;
 129
 130         if (likely(dev->flags & IFF_UP)) {
 131                 dev->stats.rx_packets++;
 132                 dev->stats.rx_bytes += skb->len;
 133                 netif_rx(skb);
 134                 skb = NULL;
 135         } else
 136                 err = -ENODEV;
 137
 138 drop:
 139         if (skb) {
 140                 dev_kfree_skb(skb);
 141                 dev->stats.rx_dropped++;
 142         }
 143         return err;
 144 }
 145
 146 static void gprs_data_ready(struct sock *sk, int len)
 147 {
 148         struct gprs_dev *gp = sk->sk_user_data;
 149         struct sk_buff *skb;
 150
 151         while ((skb = pep_read(sk)) != NULL) {
 152                 skb_orphan(skb);
 153                 gprs_recv(gp, skb);
 154         }
 155 }
 156
 157 static void gprs_write_space(struct sock *sk)
 158 {
 159         struct gprs_dev *gp = sk->sk_user_data;
 160         struct net_device *dev = gp->dev;
 161         unsigned credits = pep_writeable(sk);
 162
 163         spin_lock_bh(&gp->tx_lock);
 164         gp->tx_max = credits;
 165         if (credits > skb_queue_len(&gp->tx_queue) && netif_running(dev))
 166                 netif_wake_queue(dev);
 167         spin_unlock_bh(&gp->tx_lock);
 168 }
 169
 170 /*
 171  * Network device callbacks
 172  */
 173
 174 static int gprs_open(struct net_device *dev)
 175 {
 176         struct gprs_dev *gp = netdev_priv(dev);
 177
 178         gprs_write_space(gp->sk);
 179         return 0;
 180 }
 181
 182 static int gprs_close(struct net_device *dev)
 183 {
 184         struct gprs_dev *gp = netdev_priv(dev);
 185
 186         netif_stop_queue(dev);
 187         flush_work(&gp->tx_work);
 188         return 0;
 189 }
 190
 191 static int gprs_xmit(struct sk_buff *skb, struct net_device *dev)
 192 {
 193         struct gprs_dev *gp = netdev_priv(dev);
 194
 195         switch (skb->protocol) {
 196         case  htons(ETH_P_IP):
 197         case  htons(ETH_P_IPV6):
 198                 break;
 199         default:
 200                 dev_kfree_skb(skb);
 201                 return 0;
 202         }
 203
 204         spin_lock(&gp->tx_lock);
 205         if (likely(skb_queue_len(&gp->tx_queue) < gp->tx_max)) {
 206                 skb_queue_tail(&gp->tx_queue, skb);
 207                 skb = NULL;
 208         }
 209         if (skb_queue_len(&gp->tx_queue) >= gp->tx_max)
 210                 netif_stop_queue(dev);
 211         spin_unlock(&gp->tx_lock);
 212
 213         schedule_work(&gp->tx_work);
 214         if (unlikely(skb))
 215                 dev_kfree_skb(skb);
 216         return 0;
 217 }
 218
 219 static void gprs_tx(struct work_struct *work)
 220 {
 221         struct gprs_dev *gp = container_of(work, struct gprs_dev, tx_work);
 222         struct net_device *dev = gp->dev;
 223         struct sock *sk = gp->sk;
 224         struct sk_buff *skb;
 225
 226         while ((skb = skb_dequeue(&gp->tx_queue)) != NULL) {
 227                 int err;
 228
 229                 dev->stats.tx_bytes += skb->len;
 230                 dev->stats.tx_packets++;
 231
 232                 skb_orphan(skb);
 233                 skb_set_owner_w(skb, sk);
 234
 235                 lock_sock(sk);
 236                 err = pep_write(sk, skb);
 237                 if (err) {
 238                         LIMIT_NETDEBUG(KERN_WARNING"%s: TX error (%d)\n",
 239                                         dev->name, err);
 240                         dev->stats.tx_aborted_errors++;
 241                         dev->stats.tx_errors++;
 242                 }
 243                 release_sock(sk);
 244         }
 245
 246         lock_sock(sk);
 247         gprs_write_space(sk);
 248         release_sock(sk);
 249 }
 250
 251 static int gprs_set_mtu(struct net_device *dev, int new_mtu)
 252 {
 253         if ((new_mtu < 576) || (new_mtu > (PHONET_MAX_MTU - 11)))
 254                 return -EINVAL;
 255
 256         dev->mtu = new_mtu;
 257         return 0;
 258 }
 259
 260 static void gprs_setup(struct net_device *dev)
 261 {
 262         dev->features           = NETIF_F_FRAGLIST;
 263         dev->type               = ARPHRD_PHONET_PIPE;
 264         dev->flags              = IFF_POINTOPOINT | IFF_NOARP;
 265         dev->mtu                = GPRS_DEFAULT_MTU;
 266         dev->hard_header_len    = 0;
 267         dev->addr_len           = 0;
 268         dev->tx_queue_len       = 10;
 269
 270         dev->destructor         = free_netdev;
 271         dev->open               = gprs_open;
 272         dev->stop               = gprs_close;
 273         dev->hard_start_xmit    = gprs_xmit; /* mandatory */
 274         dev->change_mtu         = gprs_set_mtu;
 275 }
 276
 277 /*
 278  * External interface
 279  */
 280
 281 /*
 282  * Attach a GPRS interface to a datagram socket.
 283  * Returns the interface index on success, negative error code on error.
 284  */
 285 int gprs_attach(struct sock *sk)
 286 {
 287         static const char ifname[] = "gprs%d";
 288         struct gprs_dev *gp;
 289         struct net_device *dev;
 290         int err;
 291
 292         if (unlikely(sk->sk_type == SOCK_STREAM))
 293                 return -EINVAL; /* need packet boundaries */
 294
 295         /* Create net device */
 296         dev = alloc_netdev(sizeof(*gp), ifname, gprs_setup);
 297         if (!dev)
 298                 return -ENOMEM;
 299         gp = netdev_priv(dev);
 300         gp->dev = dev;
 301         gp->tx_max = 0;
 302         spin_lock_init(&gp->tx_lock);
 303         skb_queue_head_init(&gp->tx_queue);
 304         INIT_WORK(&gp->tx_work, gprs_tx);
 305
 306         netif_stop_queue(dev);
 307         err = register_netdev(dev);
 308         if (err) {
 309                 free_netdev(dev);
 310                 return err;
 311         }
 312
 313         lock_sock(sk);
 314         if (unlikely(sk->sk_user_data)) {
 315                 err = -EBUSY;
 316                 goto out_rel;
 317         }
 318         if (unlikely((1 << sk->sk_state & (TCPF_CLOSE|TCPF_LISTEN)) ||
 319                         sock_flag(sk, SOCK_DEAD))) {
 320                 err = -EINVAL;
 321                 goto out_rel;
 322         }
 323         sk->sk_user_data        = gp;
 324         gp->old_state_change    = sk->sk_state_change;
 325         gp->old_data_ready      = sk->sk_data_ready;
 326         gp->old_write_space     = sk->sk_write_space;
 327         sk->sk_state_change     = gprs_state_change;
 328         sk->sk_data_ready       = gprs_data_ready;
 329         sk->sk_write_space      = gprs_write_space;
 330         release_sock(sk);
 331
 332         sock_hold(sk);
 333         gp->sk = sk;
 334
 335         printk(KERN_DEBUG"%s: attached\n", dev->name);
 336         return dev->ifindex;
 337
 338 out_rel:
 339         release_sock(sk);
 340         unregister_netdev(dev);
 341         return err;
 342 }
 343
 344 void gprs_detach(struct sock *sk)
 345 {
 346         struct gprs_dev *gp = sk->sk_user_data;
 347         struct net_device *dev = gp->dev;
 348
 349         lock_sock(sk);
 350         sk->sk_user_data        = NULL;
 351         sk->sk_state_change     = gp->old_state_change;
 352         sk->sk_data_ready       = gp->old_data_ready;
 353         sk->sk_write_space      = gp->old_write_space;
 354         release_sock(sk);
 355
 356         printk(KERN_DEBUG"%s: detached\n", dev->name);
 357         unregister_netdev(dev);
 358         sock_put(sk);
 359 }