phonet/pep-gprs.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * File: pep-gprs.c
   4  *
   5  * GPRS over Phonet pipe end point socket
   6  *
   7  * Copyright (C) 2008 Nokia Corporation.
   8  *
   9  * Author: Rémi Denis-Courmont
  10  */
  11
  12 #include <linux/kernel.h>
  13 #include <linux/netdevice.h>
  14 #include <linux/if_ether.h>
  15 #include <linux/if_arp.h>
  16 #include <net/sock.h>
  17
  18 #include <linux/if_phonet.h>
  19 #include <net/tcp_states.h>
  20 #include <net/phonet/gprs.h>
  21
  22 #include <trace/events/sock.h>
  23
  24 #define GPRS_DEFAULT_MTU 1400
  25
  26 struct gprs_dev {
  27         struct sock             *sk;
  28         void                    (*old_state_change)(struct sock *);
  29         void                    (*old_data_ready)(struct sock *);
  30         void                    (*old_write_space)(struct sock *);
  31
  32         struct net_device       *dev;
  33 };
  34
  35 static __be16 gprs_type_trans(struct sk_buff *skb)
  36 {
  37         const u8 *pvfc;
  38         u8 buf;
  39
  40         pvfc = skb_header_pointer(skb, 0, 1, &buf);
  41         if (!pvfc)
  42                 return htons(0);
  43         /* Look at IP version field */
  44         switch (*pvfc >> 4) {
  45         case 4:
  46                 return htons(ETH_P_IP);
  47         case 6:
  48                 return htons(ETH_P_IPV6);
  49         }
  50         return htons(0);
  51 }
  52
  53 static void gprs_writeable(struct gprs_dev *gp)
  54 {
  55         struct net_device *dev = gp->dev;
  56
  57         if (pep_writeable(gp->sk))
  58                 netif_wake_queue(dev);
  59 }
  60
  61 /*
  62  * Socket callbacks
  63  */
  64
  65 static void gprs_state_change(struct sock *sk)
  66 {
  67         struct gprs_dev *gp = sk->sk_user_data;
  68
  69         if (sk->sk_state == TCP_CLOSE_WAIT) {
  70                 struct net_device *dev = gp->dev;
  71
  72                 netif_stop_queue(dev);
  73                 netif_carrier_off(dev);
  74         }
  75 }
  76
  77 static int gprs_recv(struct gprs_dev *gp, struct sk_buff *skb)
  78 {
  79         struct net_device *dev = gp->dev;
  80         int err = 0;
  81         __be16 protocol = gprs_type_trans(skb);
  82
  83         if (!protocol) {
  84                 err = -EINVAL;
  85                 goto drop;
  86         }
  87
  88         if (skb_headroom(skb) & 3) {
  89                 struct sk_buff *rskb, *fs;
  90                 int flen = 0;
  91
  92                 /* Phonet Pipe data header may be misaligned (3 bytes),
  93                  * so wrap the IP packet as a single fragment of an head-less
  94                  * socket buffer. The network stack will pull what it needs,
  95                  * but at least, the whole IP payload is not memcpy'd. */
  96                 rskb = netdev_alloc_skb(dev, 0);
  97                 if (!rskb) {
  98                         err = -ENOBUFS;
  99                         goto drop;
 100                 }
 101                 skb_shinfo(rskb)->frag_list = skb;
 102                 rskb->len += skb->len;
 103                 rskb->data_len += rskb->len;
 104                 rskb->truesize += rskb->len;
 105
 106                 /* Avoid nested fragments */
 107                 skb_walk_frags(skb, fs)
 108                         flen += fs->len;
 109                 skb->next = skb_shinfo(skb)->frag_list;
 110                 skb_frag_list_init(skb);
 111                 skb->len -= flen;
 112                 skb->data_len -= flen;
 113                 skb->truesize -= flen;
 114
 115                 skb = rskb;
 116         }
 117
 118         skb->protocol = protocol;
 119         skb_reset_mac_header(skb);
 120         skb->dev = dev;
 121
 122         if (likely(dev->flags & IFF_UP)) {
 123                 dev->stats.rx_packets++;
 124                 dev->stats.rx_bytes += skb->len;
 125                 netif_rx(skb);
 126                 skb = NULL;
 127         } else
 128                 err = -ENODEV;
 129
 130 drop:
 131         if (skb) {
 132                 dev_kfree_skb(skb);
 133                 dev->stats.rx_dropped++;
 134         }
 135         return err;
 136 }
 137
 138 static void gprs_data_ready(struct sock *sk)
 139 {
 140         struct gprs_dev *gp = sk->sk_user_data;
 141         struct sk_buff *skb;
 142
 143         trace_sk_data_ready(sk);
 144
 145         while ((skb = pep_read(sk)) != NULL) {
 146                 skb_orphan(skb);
 147                 gprs_recv(gp, skb);
 148         }
 149 }
 150
 151 static void gprs_write_space(struct sock *sk)
 152 {
 153         struct gprs_dev *gp = sk->sk_user_data;
 154
 155         if (netif_running(gp->dev))
 156                 gprs_writeable(gp);
 157 }
 158
 159 /*
 160  * Network device callbacks
 161  */
 162
 163 static int gprs_open(struct net_device *dev)
 164 {
 165         struct gprs_dev *gp = netdev_priv(dev);
 166
 167         gprs_writeable(gp);
 168         return 0;
 169 }
 170
 171 static int gprs_close(struct net_device *dev)
 172 {
 173         netif_stop_queue(dev);
 174         return 0;
 175 }
 176
 177 static netdev_tx_t gprs_xmit(struct sk_buff *skb, struct net_device *dev)
 178 {
 179         struct gprs_dev *gp = netdev_priv(dev);
 180         struct sock *sk = gp->sk;
 181         int len, err;
 182
 183         switch (skb->protocol) {
 184         case  htons(ETH_P_IP):
 185         case  htons(ETH_P_IPV6):
 186                 break;
 187         default:
 188                 dev_kfree_skb(skb);
 189                 return NETDEV_TX_OK;
 190         }
 191
 192         skb_orphan(skb);
 193         skb_set_owner_w(skb, sk);
 194         len = skb->len;
 195         err = pep_write(sk, skb);
 196         if (err) {
 197                 net_dbg_ratelimited("%s: TX error (%d)\n", dev->name, err);
 198                 dev->stats.tx_aborted_errors++;
 199                 dev->stats.tx_errors++;
 200         } else {
 201                 dev->stats.tx_packets++;
 202                 dev->stats.tx_bytes += len;
 203         }
 204
 205         netif_stop_queue(dev);
 206         if (pep_writeable(sk))
 207                 netif_wake_queue(dev);
 208         return NETDEV_TX_OK;
 209 }
 210
 211 static const struct net_device_ops gprs_netdev_ops = {
 212         .ndo_open       = gprs_open,
 213         .ndo_stop       = gprs_close,
 214         .ndo_start_xmit = gprs_xmit,
 215 };
 216
 217 static void gprs_setup(struct net_device *dev)
 218 {
 219         dev->features           = NETIF_F_FRAGLIST;
 220         dev->type               = ARPHRD_PHONET_PIPE;
 221         dev->flags              = IFF_POINTOPOINT | IFF_NOARP;
 222         dev->mtu                = GPRS_DEFAULT_MTU;
 223         dev->min_mtu            = 576;
 224         dev->max_mtu            = (PHONET_MAX_MTU - 11);
 225         dev->hard_header_len    = 0;
 226         dev->addr_len           = 0;
 227         dev->tx_queue_len       = 10;
 228
 229         dev->netdev_ops         = &gprs_netdev_ops;
 230         dev->needs_free_netdev  = true;
 231 }
 232
 233 /*
 234  * External interface
 235  */
 236
 237 /*
 238  * Attach a GPRS interface to a datagram socket.
 239  * Returns the interface index on success, negative error code on error.
 240  */
 241 int gprs_attach(struct sock *sk)
 242 {
 243         static const char ifname[] = "gprs%d";
 244         struct gprs_dev *gp;
 245         struct net_device *dev;
 246         int err;
 247
 248         if (unlikely(sk->sk_type == SOCK_STREAM))
 249                 return -EINVAL; /* need packet boundaries */
 250
 251         /* Create net device */
 252         dev = alloc_netdev(sizeof(*gp), ifname, NET_NAME_UNKNOWN, gprs_setup);
 253         if (!dev)
 254                 return -ENOMEM;
 255         gp = netdev_priv(dev);
 256         gp->sk = sk;
 257         gp->dev = dev;
 258
 259         netif_stop_queue(dev);
 260         err = register_netdev(dev);
 261         if (err) {
 262                 free_netdev(dev);
 263                 return err;
 264         }
 265
 266         lock_sock(sk);
 267         if (unlikely(sk->sk_user_data)) {
 268                 err = -EBUSY;
 269                 goto out_rel;
 270         }
 271         if (unlikely((1 << sk->sk_state & (TCPF_CLOSE|TCPF_LISTEN)) ||
 272                         sock_flag(sk, SOCK_DEAD))) {
 273                 err = -EINVAL;
 274                 goto out_rel;
 275         }
 276         sk->sk_user_data        = gp;
 277         gp->old_state_change    = sk->sk_state_change;
 278         gp->old_data_ready      = sk->sk_data_ready;
 279         gp->old_write_space     = sk->sk_write_space;
 280         sk->sk_state_change     = gprs_state_change;
 281         sk->sk_data_ready       = gprs_data_ready;
 282         sk->sk_write_space      = gprs_write_space;
 283         release_sock(sk);
 284         sock_hold(sk);
 285
 286         printk(KERN_DEBUG"%s: attached\n", dev->name);
 287         return dev->ifindex;
 288
 289 out_rel:
 290         release_sock(sk);
 291         unregister_netdev(dev);
 292         return err;
 293 }
 294
 295 void gprs_detach(struct sock *sk)
 296 {
 297         struct gprs_dev *gp = sk->sk_user_data;
 298         struct net_device *dev = gp->dev;
 299
 300         lock_sock(sk);
 301         sk->sk_user_data        = NULL;
 302         sk->sk_state_change     = gp->old_state_change;
 303         sk->sk_data_ready       = gp->old_data_ready;
 304         sk->sk_write_space      = gp->old_write_space;
 305         release_sock(sk);
 306
 307         printk(KERN_DEBUG"%s: detached\n", dev->name);
 308         unregister_netdev(dev);
 309         sock_put(sk);
 310 }