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       *net;
  44         struct net_device_stats stats;
  45
  46         struct sk_buff_head     tx_queue;
  47         struct work_struct      tx_work;
  48         spinlock_t              tx_lock;
  49         unsigned                tx_max;
  50 };
  51
  52 static int gprs_type_trans(struct sk_buff *skb)
  53 {
  54         const u8 *pvfc;
  55         u8 buf;
  56
  57         pvfc = skb_header_pointer(skb, 0, 1, &buf);
  58         if (!pvfc)
  59                 return 0;
  60         /* Look at IP version field */
  61         switch (*pvfc >> 4) {
  62         case 4:
  63                 return htons(ETH_P_IP);
  64         case 6:
  65                 return htons(ETH_P_IPV6);
  66         }
  67         return 0;
  68 }
  69
  70 /*
  71  * Socket callbacks
  72  */
  73
  74 static void gprs_state_change(struct sock *sk)
  75 {
  76         struct gprs_dev *dev = sk->sk_user_data;
  77
  78         if (sk->sk_state == TCP_CLOSE_WAIT) {
  79                 netif_stop_queue(dev->net);
  80                 netif_carrier_off(dev->net);
  81         }
  82 }
  83
  84 static int gprs_recv(struct gprs_dev *dev, struct sk_buff *skb)
  85 {
  86         int err = 0;
  87         u16 protocol = gprs_type_trans(skb);
  88
  89         if (!protocol) {
  90                 err = -EINVAL;
  91                 goto drop;
  92         }
  93
  94         if (likely(skb_headroom(skb) & 3)) {
  95                 struct sk_buff *rskb, *fs;
  96                 int flen = 0;
  97
  98                 /* Phonet Pipe data header is misaligned (3 bytes),
  99                  * so wrap the IP packet as a single fragment of an head-less
 100                  * socket buffer. The network stack will pull what it needs,
 101                  * but at least, the whole IP payload is not memcpy'd. */
 102                 rskb = netdev_alloc_skb(dev->net, 0);
 103                 if (!rskb) {
 104                         err = -ENOBUFS;
 105                         goto drop;
 106                 }
 107                 skb_shinfo(rskb)->frag_list = skb;
 108                 rskb->len += skb->len;
 109                 rskb->data_len += rskb->len;
 110                 rskb->truesize += rskb->len;
 111
 112                 /* Avoid nested fragments */
 113                 for (fs = skb_shinfo(skb)->frag_list; fs; fs = fs->next)
 114                         flen += fs->len;
 115                 skb->next = skb_shinfo(skb)->frag_list;
 116                 skb_shinfo(skb)->frag_list = NULL;
 117                 skb->len -= flen;
 118                 skb->data_len -= flen;
 119                 skb->truesize -= flen;
 120
 121                 skb = rskb;
 122         }
 123
 124         skb->protocol = protocol;
 125         skb_reset_mac_header(skb);
 126         skb->dev = dev->net;
 127
 128         if (likely(dev->net->flags & IFF_UP)) {
 129                 dev->stats.rx_packets++;
 130                 dev->stats.rx_bytes += skb->len;
 131                 netif_rx(skb);
 132                 skb = NULL;
 133         } else
 134                 err = -ENODEV;
 135
 136 drop:
 137         if (skb) {
 138                 dev_kfree_skb(skb);
 139                 dev->stats.rx_dropped++;
 140         }
 141         return err;
 142 }
 143
 144 static void gprs_data_ready(struct sock *sk, int len)
 145 {
 146         struct gprs_dev *dev = sk->sk_user_data;
 147         struct sk_buff *skb;
 148
 149         while ((skb = pep_read(sk)) != NULL) {
 150                 skb_orphan(skb);
 151                 gprs_recv(dev, skb);
 152         }
 153 }
 154
 155 static void gprs_write_space(struct sock *sk)
 156 {
 157         struct gprs_dev *dev = sk->sk_user_data;
 158         struct net_device *net = dev->net;
 159         unsigned credits = pep_writeable(sk);
 160
 161         spin_lock_bh(&dev->tx_lock);
 162         dev->tx_max = credits;
 163         if (credits > skb_queue_len(&dev->tx_queue) && netif_running(net))
 164                 netif_wake_queue(net);
 165         spin_unlock_bh(&dev->tx_lock);
 166 }
 167
 168 /*
 169  * Network device callbacks
 170  */
 171
 172 static int gprs_open(struct net_device *dev)
 173 {
 174         struct gprs_dev *gp = netdev_priv(dev);
 175
 176         gprs_write_space(gp->sk);
 177         return 0;
 178 }
 179
 180 static int gprs_close(struct net_device *dev)
 181 {
 182         struct gprs_dev *gp = netdev_priv(dev);
 183
 184         netif_stop_queue(dev);
 185         flush_work(&gp->tx_work);
 186         return 0;
 187 }
 188
 189 static int gprs_xmit(struct sk_buff *skb, struct net_device *net)
 190 {
 191         struct gprs_dev *dev = netdev_priv(net);
 192
 193         switch (skb->protocol) {
 194         case  htons(ETH_P_IP):
 195         case  htons(ETH_P_IPV6):
 196                 break;
 197         default:
 198                 dev_kfree_skb(skb);
 199                 return 0;
 200         }
 201
 202         spin_lock(&dev->tx_lock);
 203         if (likely(skb_queue_len(&dev->tx_queue) < dev->tx_max)) {
 204                 skb_queue_tail(&dev->tx_queue, skb);
 205                 skb = NULL;
 206         }
 207         if (skb_queue_len(&dev->tx_queue) >= dev->tx_max)
 208                 netif_stop_queue(net);
 209         spin_unlock(&dev->tx_lock);
 210
 211         schedule_work(&dev->tx_work);
 212         if (unlikely(skb))
 213                 dev_kfree_skb(skb);
 214         return 0;
 215 }
 216
 217 static void gprs_tx(struct work_struct *work)
 218 {
 219         struct gprs_dev *dev = container_of(work, struct gprs_dev, tx_work);
 220         struct sock *sk = dev->sk;
 221         struct sk_buff *skb;
 222
 223         while ((skb = skb_dequeue(&dev->tx_queue)) != NULL) {
 224                 int err;
 225
 226                 dev->stats.tx_bytes += skb->len;
 227                 dev->stats.tx_packets++;
 228
 229                 skb_orphan(skb);
 230                 skb_set_owner_w(skb, sk);
 231
 232                 lock_sock(sk);
 233                 err = pep_write(sk, skb);
 234                 if (err) {
 235                         LIMIT_NETDEBUG(KERN_WARNING"%s: TX error (%d)\n",
 236                                         dev->net->name, err);
 237                         dev->stats.tx_aborted_errors++;
 238                         dev->stats.tx_errors++;
 239                 }
 240                 release_sock(sk);
 241         }
 242
 243         lock_sock(sk);
 244         gprs_write_space(sk);
 245         release_sock(sk);
 246 }
 247
 248 static int gprs_set_mtu(struct net_device *net, int new_mtu)
 249 {
 250         if ((new_mtu < 576) || (new_mtu > (PHONET_MAX_MTU - 11)))
 251                 return -EINVAL;
 252
 253         net->mtu = new_mtu;
 254         return 0;
 255 }
 256
 257 static struct net_device_stats *gprs_get_stats(struct net_device *net)
 258 {
 259         struct gprs_dev *dev = netdev_priv(net);
 260
 261         return &dev->stats;
 262 }
 263
 264 static void gprs_setup(struct net_device *net)
 265 {
 266         net->features           = NETIF_F_FRAGLIST;
 267         net->type               = ARPHRD_NONE;
 268         net->flags              = IFF_POINTOPOINT | IFF_NOARP;
 269         net->mtu                = GPRS_DEFAULT_MTU;
 270         net->hard_header_len    = 0;
 271         net->addr_len           = 0;
 272         net->tx_queue_len       = 10;
 273
 274         net->destructor         = free_netdev;
 275         net->open               = gprs_open;
 276         net->stop               = gprs_close;
 277         net->hard_start_xmit    = gprs_xmit; /* mandatory */
 278         net->change_mtu         = gprs_set_mtu;
 279         net->get_stats          = gprs_get_stats;
 280 }
 281
 282 /*
 283  * External interface
 284  */
 285
 286 /*
 287  * Attach a GPRS interface to a datagram socket.
 288  * Returns the interface index on success, negative error code on error.
 289  */
 290 int gprs_attach(struct sock *sk)
 291 {
 292         static const char ifname[] = "gprs%d";
 293         struct gprs_dev *dev;
 294         struct net_device *net;
 295         int err;
 296
 297         if (unlikely(sk->sk_type == SOCK_STREAM))
 298                 return -EINVAL; /* need packet boundaries */
 299
 300         /* Create net device */
 301         net = alloc_netdev(sizeof(*dev), ifname, gprs_setup);
 302         if (!net)
 303                 return -ENOMEM;
 304         dev = netdev_priv(net);
 305         dev->net = net;
 306         dev->tx_max = 0;
 307         spin_lock_init(&dev->tx_lock);
 308         skb_queue_head_init(&dev->tx_queue);
 309         INIT_WORK(&dev->tx_work, gprs_tx);
 310
 311         netif_stop_queue(net);
 312         err = register_netdev(net);
 313         if (err) {
 314                 free_netdev(net);
 315                 return err;
 316         }
 317
 318         lock_sock(sk);
 319         if (unlikely(sk->sk_user_data)) {
 320                 err = -EBUSY;
 321                 goto out_rel;
 322         }
 323         if (unlikely((1 << sk->sk_state & (TCPF_CLOSE|TCPF_LISTEN)) ||
 324                         sock_flag(sk, SOCK_DEAD))) {
 325                 err = -EINVAL;
 326                 goto out_rel;
 327         }
 328         sk->sk_user_data        = dev;
 329         dev->old_state_change   = sk->sk_state_change;
 330         dev->old_data_ready     = sk->sk_data_ready;
 331         dev->old_write_space    = sk->sk_write_space;
 332         sk->sk_state_change     = gprs_state_change;
 333         sk->sk_data_ready       = gprs_data_ready;
 334         sk->sk_write_space      = gprs_write_space;
 335         release_sock(sk);
 336
 337         sock_hold(sk);
 338         dev->sk = sk;
 339
 340         printk(KERN_DEBUG"%s: attached\n", net->name);
 341         return net->ifindex;
 342
 343 out_rel:
 344         release_sock(sk);
 345         unregister_netdev(net);
 346         return err;
 347 }
 348
 349 void gprs_detach(struct sock *sk)
 350 {
 351         struct gprs_dev *dev = sk->sk_user_data;
 352         struct net_device *net = dev->net;
 353
 354         lock_sock(sk);
 355         sk->sk_user_data        = NULL;
 356         sk->sk_state_change     = dev->old_state_change;
 357         sk->sk_data_ready       = dev->old_data_ready;
 358         sk->sk_write_space      = dev->old_write_space;
 359         release_sock(sk);
 360
 361         printk(KERN_DEBUG"%s: detached\n", net->name);
 362         unregister_netdev(net);
 363         sock_put(sk);
 364 }