pep-gprs.c 6.0 KB

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