SHARE
TWEET

Untitled

a guest May 20th, 2019 62 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // SPDX-License-Identifier: (GPL-2.0 WITH Linux-syscall-note) AND MIT
  2. /*
  3.  * af_stp.c - Linux kernel list API
  4.  *
  5.  * Authors: Octavian Guzu <octavian.guzu@gmail.com>
  6.  *      Liza-Elena Babu <lizza.babu@gmail.com>
  7.  */
  8.  
  9. #include "stp.h"
  10. #include "af_stp.h"
  11.  
  12. DEFINE_RWLOCK(lock);
  13. DEFINE_HASHTABLE(stp_hash, HT_SIZE);
  14. struct proc_dir_entry *proc_list_read;
  15. struct stp_stats global_stats;
  16.  
  17. static __u8 get_csum(char *start, char *end)
  18. {
  19.     __u8 res = 0;
  20.  
  21.     while (start != end) {
  22.         res ^= *start;
  23.         start++;
  24.     }
  25.  
  26.     return res;
  27. }
  28.  
  29. static int stp_release(struct socket *sock)
  30. {
  31.     struct stp_sock *s_stp;
  32.     struct sock *sk;
  33.  
  34.     sk = sock->sk;
  35.     s_stp = (struct stp_sock *)sk;
  36.  
  37.     if (!sk)
  38.         return 0;
  39.  
  40.     if (s_stp->hash_node) {
  41.         write_lock(&lock);
  42.         hash_del(&s_stp->hash_node->next);
  43.         kfree(s_stp->hash_node);
  44.         write_unlock(&lock);
  45.     }
  46.  
  47.     sock_orphan(sk);
  48.     sock->sk = NULL;
  49.  
  50.     sock_put(sk);
  51.  
  52.     return 0;
  53. }
  54.  
  55. static int stp_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len)
  56. {
  57.     struct sockaddr_stp *sstp = (struct sockaddr_stp *) uaddr;
  58.     struct stp_sock *stp_sock;
  59.     struct hash_elem *elem = NULL;
  60.     struct hlist_node *tmp = NULL;
  61.     struct hash_elem *he = NULL;
  62.     __be16 port;
  63.     int err;
  64.  
  65.     err = -EINVAL;
  66.     if (addr_len < sizeof(struct sockaddr_stp))
  67.         goto out_err;
  68.  
  69.     err = -EINVAL;
  70.     if (sstp->sas_family != AF_STP)
  71.         goto out_err;
  72.  
  73.     err = -EINVAL;
  74.     port = ntohs(sstp->sas_port);
  75.     if (port < 1 || port > 65535)
  76.         goto out_err;
  77.  
  78.     stp_sock = (struct stp_sock *) sock->sk;
  79.     stp_sock->port_src = sstp->sas_port;
  80.     stp_sock->if_idx = sstp->sas_ifindex;
  81.     memcpy(stp_sock->mac_addr_src, sstp->sas_addr, MAC_ADDR_SIZE);
  82.  
  83.     read_lock(&lock);
  84.     hash_for_each_possible_safe(stp_hash, he, tmp, next,
  85.             stp_sock->port_src) {
  86.         err = -EINVAL;
  87.         read_unlock(&lock);
  88.         goto out_err;
  89.     }
  90.     read_unlock(&lock);
  91.  
  92.     err = -ENOMEM;
  93.     elem = kmalloc(sizeof(*elem), GFP_KERNEL);
  94.     if (!elem)
  95.         goto out_err;
  96.  
  97.     elem->hd.if_idx = stp_sock->if_idx;
  98.     elem->hd.port = stp_sock->port_src;
  99.     elem->hd.socket = stp_sock;
  100.     stp_sock->hash_node = elem;
  101.  
  102.     write_lock(&lock);
  103.     hash_add(stp_hash, &elem->next, stp_sock->port_src);
  104.     write_unlock(&lock);
  105.  
  106.     return 0;
  107.  
  108. out_err:
  109.     return err;
  110.  
  111. }
  112.  
  113. static int stp_connect(struct socket *sock, struct sockaddr *vaddr,
  114.         int sockaddr_len, int flags)
  115. {
  116.     struct stp_sock *sstp;
  117.     struct sockaddr_stp *addr;
  118.     int sockaddr_stp_size;
  119.  
  120.     sstp = (struct stp_sock *)sock->sk;
  121.     addr = (struct sockaddr_stp *)vaddr;
  122.     sockaddr_stp_size = sizeof(struct sockaddr_stp);
  123.  
  124.     if (sockaddr_len != sockaddr_stp_size || addr->sas_family != AF_STP)
  125.         return -EINVAL;
  126.  
  127.     memcpy(sstp->mac_addr_dst, addr->sas_addr, MAC_ADDR_SIZE);
  128.     sstp->port_dst = addr->sas_port;
  129.  
  130.     return 0;
  131. }
  132.  
  133. static int stp_sendmsg(struct socket *sock, struct msghdr *msg, size_t lenght)
  134. {
  135.     struct sockaddr_stp *addr_s;
  136.     struct net_device *dev;
  137.     struct stp_sock *stp_s;
  138.     struct stp_hdr *hdr_s;
  139.     struct sk_buff *skb;
  140.     int offset_e, hdr_len, err, flags;
  141.     __u8 *mac;
  142.     __be16 port;
  143.  
  144.     err = -EINVAL;
  145.     stp_s = (struct stp_sock *) sock->sk;
  146.     dev = dev_get_by_index(sock_net(sock->sk), stp_s->if_idx);
  147.     if (!dev)
  148.         goto out;
  149.  
  150.     hdr_len = sizeof(struct stp_hdr) + lenght;
  151.     flags = msg->msg_flags & MSG_DONTWAIT;
  152.     skb = sock_alloc_send_pskb(sock->sk, LL_RESERVED_SPACE(dev),
  153.             hdr_len, flags, &err, 0);
  154.     if (!skb)
  155.         goto out;
  156.  
  157.     skb_reserve(skb, LL_RESERVED_SPACE(dev));
  158.  
  159.     addr_s = (struct sockaddr_stp *) msg->msg_name;
  160.     port = stp_s->port_dst ? stp_s->port_dst : addr_s->sas_port;
  161.     mac = stp_s->port_dst ? stp_s->mac_addr_dst : addr_s->sas_addr;
  162.  
  163.     err = dev_hard_header(skb, dev, ETH_P_STP, mac,
  164.             stp_s->mac_addr_src, hdr_len);
  165.     offset_e = err;
  166.     pr_alert("****** %d\n", offset_e);
  167.     err = -EINVAL;
  168.     hdr_s = (struct stp_hdr *) skb_put(skb, sizeof(struct stp_hdr));
  169.     if (!hdr_s)
  170.         goto out;
  171.  
  172.     hdr_s->len = htons(hdr_len);
  173.     hdr_s->src = stp_s->port_src;
  174.     hdr_s->dst = port;
  175.     hdr_s->csum = 0;
  176.  
  177.     skb_put(skb, lenght);
  178.  
  179.     err = skb_copy_datagram_from_iter(skb,
  180.             offset_e + sizeof(struct stp_hdr),
  181.             &msg->msg_iter, lenght);
  182.     if (err)
  183.         goto out;
  184.  
  185.     hdr_s->csum = get_csum(skb->data + offset_e, skb->tail);
  186.  
  187.     skb->dev = dev;
  188.     skb->protocol = htons(ETH_P_STP);
  189.     skb->priority = sock->sk->sk_priority;
  190.  
  191.     err = dev_queue_xmit(skb);
  192.     if (err)
  193.         goto out;
  194.  
  195.     global_stats.txpkts++;
  196.  
  197.     dev_put(dev);
  198.     err = lenght;
  199.  
  200. out:
  201.     return err;
  202. }
  203.  
  204. static int stp_recvmsg(struct socket *sock, struct msghdr *msg,
  205.         size_t len, int flags)
  206. {
  207.     struct sockaddr_stp *addr_s;
  208.     struct stp_sock *stp_s;
  209.     struct stp_hdr *hdr_s;
  210.     struct sk_buff *skb;
  211.     struct ethhdr *eth;
  212.     int err;
  213.  
  214.     skb = skb_recv_datagram(sock->sk, flags, flags & MSG_DONTWAIT, &err);
  215.     err = -EINVAL;
  216.     if (!skb)
  217.         goto out;
  218.  
  219.     err = -EINVAL;
  220.     if (get_csum(skb->data, skb->tail)) {
  221.         global_stats.csumerr++;
  222.         goto skb_release_datagram;
  223.     }
  224.  
  225.     err = skb_copy_datagram_iter(skb, sizeof(struct stp_hdr),
  226.             &msg->msg_iter, len);
  227.     if (err)
  228.         goto skb_release_datagram;
  229.  
  230.     addr_s = (struct sockaddr_stp *) msg->msg_name;
  231.     stp_s = (struct stp_sock *) sock->sk;
  232.     hdr_s = (struct stp_hdr *) skb->data;
  233.     eth = eth_hdr(skb);
  234.     if (eth && addr_s) {
  235.         addr_s->sas_port = hdr_s->src;
  236.         addr_s->sas_family = AF_STP;
  237.         memcpy(addr_s->sas_addr, eth->h_source, MAC_ADDR_SIZE);
  238.     }
  239.  
  240.     sock_recv_ts_and_drops(msg, sock->sk, skb);
  241.  
  242.     global_stats.rxpkts++;
  243.     err = len;
  244.  
  245. skb_release_datagram:
  246.     skb_free_datagram(sock->sk, skb);
  247. out:
  248.     return err;
  249. }
  250.  
  251. static int stp_packet_recv(struct sk_buff *skb, struct net_device *dev,
  252.         struct packet_type *p_type, struct net_device *orig_dev)
  253. {
  254.     struct stp_hdr *stp_header;
  255.     struct hlist_node *tmp = NULL;
  256.     struct hash_elem *he = NULL;
  257.     struct stp_sock *stp_s = NULL;
  258.     __be16 dst, src;
  259.     int err;
  260.  
  261.     err = -EINVAL;
  262.     if (skb->len < sizeof(struct stp_hdr)) {
  263.         global_stats.hdrerr++;
  264.         goto out;
  265.     }
  266.  
  267.     stp_header = (struct stp_hdr *) skb->data;
  268.     dst = stp_header->dst;
  269.     src = stp_header->src;
  270.  
  271.     err = -EINVAL;
  272.     if (!dst || !src) {
  273.         global_stats.hdrerr++;
  274.         goto out;
  275.     }
  276.  
  277.     read_lock(&lock);
  278.     hash_for_each_possible_safe(stp_hash, he, tmp, next, dst) {
  279.         stp_s = he->hd.socket;
  280.     }
  281.     read_unlock(&lock);
  282.  
  283.     err = -EINVAL;
  284.     if (stp_s == NULL) {
  285.         global_stats.nosock++;
  286.         goto out;
  287.     }
  288.  
  289.     err = sock_queue_rcv_skb(&stp_s->sk, skb);
  290.     if (err) {
  291.         global_stats.nobuffs++;
  292.         goto out;
  293.     }
  294.  
  295.     return 0;
  296. out:
  297.     return err;
  298. }
  299.  
  300. static int stp_packet_create(struct net *net, struct socket *sock,
  301.         int protocol, int kern)
  302. {
  303.  
  304.     struct sock *sk;
  305.     int err;
  306.  
  307.     if (sock->type != SOCK_DGRAM)
  308.         return -ESOCKTNOSUPPORT;
  309.  
  310.     err = -EINVAL;
  311.     if (protocol)
  312.         goto out;
  313.  
  314.     sock->state = SS_UNCONNECTED;
  315.  
  316.     err = -ENOBUFS;
  317.     sk = sk_alloc(net, PF_STP, GFP_KERNEL, &stp_proto, kern);
  318.     if (!sk)
  319.         goto out;
  320.  
  321.     sock->ops = &stp_proto_ops;
  322.  
  323.     sock_init_data(sock, sk);
  324.  
  325.     sk->sk_family = PF_STP;
  326.  
  327.     return 0;
  328. out:
  329.     return err;
  330. }
  331.  
  332. static int stp_proc_show(struct seq_file *m, void *v)
  333. {
  334.     seq_puts(m, "RxPkts HdrErr CsumErr NoSock NoBuffs TxPkts\n");
  335.  
  336.     seq_printf(m, "%d %d %d %d %d %d\n",
  337.             global_stats.rxpkts,
  338.             global_stats.hdrerr,
  339.             global_stats.csumerr,
  340.             global_stats.nosock,
  341.             global_stats.nobuffs,
  342.             global_stats.txpkts);
  343.  
  344.     return 0;
  345. }
  346.  
  347. static int stp_read_open(struct inode *inode, struct  file *file)
  348. {
  349.     return single_open(file, stp_proc_show, NULL);
  350. }
  351.  
  352. static const struct file_operations r_fops = {
  353.     .owner      = THIS_MODULE,
  354.     .open       = stp_read_open,
  355.     .read       = seq_read,
  356.     .release    = single_release,
  357. };
  358.  
  359. static int stp_init(void)
  360. {
  361.     int err;
  362.  
  363.     err = -ENOMEM;
  364.     proc_list_read = proc_create(STP_PROC_NET_FILENAME, 0000,
  365.             init_net.proc_net, &r_fops);
  366.     if (!proc_list_read)
  367.         goto out;
  368.  
  369.     err = proto_register(&stp_proto, 0);
  370.     if (err)
  371.         goto proto_reg_fail;
  372.  
  373.     err = sock_register(&stp_packet_family_ops);
  374.     if (err)
  375.         goto sock_reg_fail;
  376.  
  377.     dev_add_pack(&stp_packet_type);
  378.  
  379.     return 0;
  380.  
  381. sock_reg_fail:
  382.     proto_unregister(&stp_proto);
  383. proto_reg_fail:
  384.     proc_remove(proc_list_read);
  385. out:
  386.     return err;
  387. }
  388.  
  389. static void stp_exit(void)
  390. {
  391.     struct hlist_node *tmp = NULL;
  392.     struct hash_elem *he = NULL;
  393.     int count = 0;
  394.  
  395.     dev_remove_pack(&stp_packet_type);
  396.     proc_remove(proc_list_read);
  397.     proto_unregister(&stp_proto);
  398.     sock_unregister(PF_STP);
  399.  
  400.     write_lock(&lock);
  401.     hash_for_each_safe(stp_hash, count, tmp, he, next) {
  402.         hash_del(tmp);
  403.         kfree(he);
  404.     }
  405.     write_unlock(&lock);
  406. }
  407.  
  408. module_init(stp_init);
  409. module_exit(stp_exit);
  410.  
  411. MODULE_DESCRIPTION("Linux kernel networking protocol");
  412. MODULE_AUTHOR("Octavian Guzu & Liza Babu");
  413. MODULE_LICENSE("GPL v2");
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top