drivers/staging/gdm724x/netlink_k.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */
   3
   4 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   5
   6 #include <linux/export.h>
   7 #include <linux/mutex.h>
   8 #include <linux/etherdevice.h>
   9 #include <linux/netlink.h>
  10 #include <asm/byteorder.h>
  11 #include <net/sock.h>
  12
  13 #include "netlink_k.h"
  14
  15 static DEFINE_MUTEX(netlink_mutex);
  16
  17 #define ND_MAX_GROUP            30
  18 #define ND_IFINDEX_LEN          sizeof(int)
  19 #define ND_NLMSG_SPACE(len)     (NLMSG_SPACE(len) + ND_IFINDEX_LEN)
  20 #define ND_NLMSG_DATA(nlh)      ((void *)((char *)NLMSG_DATA(nlh) + \
  21                                                   ND_IFINDEX_LEN))
  22 #define ND_NLMSG_S_LEN(len)     (len + ND_IFINDEX_LEN)
  23 #define ND_NLMSG_R_LEN(nlh)     (nlh->nlmsg_len - ND_IFINDEX_LEN)
  24 #define ND_NLMSG_IFIDX(nlh)     NLMSG_DATA(nlh)
  25 #define ND_MAX_MSG_LEN          (1024 * 32)
  26
  27 static void (*rcv_cb)(struct net_device *dev, u16 type, void *msg, int len);
  28
  29 static void netlink_rcv_cb(struct sk_buff *skb)
  30 {
  31         struct nlmsghdr *nlh;
  32         struct net_device *dev;
  33         u32 mlen;
  34         void *msg;
  35         int ifindex;
  36
  37         if (!rcv_cb) {
  38                 pr_err("nl cb - unregistered\n");
  39                 return;
  40         }
  41
  42         if (skb->len < NLMSG_HDRLEN) {
  43                 pr_err("nl cb - invalid skb length\n");
  44                 return;
  45         }
  46
  47         nlh = (struct nlmsghdr *)skb->data;
  48
  49         if (skb->len < nlh->nlmsg_len || nlh->nlmsg_len > ND_MAX_MSG_LEN) {
  50                 pr_err("nl cb - invalid length (%d,%d)\n",
  51                        skb->len, nlh->nlmsg_len);
  52                 return;
  53         }
  54
  55         memcpy(&ifindex, ND_NLMSG_IFIDX(nlh), ND_IFINDEX_LEN);
  56         msg = ND_NLMSG_DATA(nlh);
  57         mlen = ND_NLMSG_R_LEN(nlh);
  58
  59         dev = dev_get_by_index(&init_net, ifindex);
  60         if (dev) {
  61                 rcv_cb(dev, nlh->nlmsg_type, msg, mlen);
  62                 dev_put(dev);
  63         } else {
  64                 pr_err("nl cb - dev (%d) not found\n", ifindex);
  65         }
  66 }
  67
  68 static void netlink_rcv(struct sk_buff *skb)
  69 {
  70         mutex_lock(&netlink_mutex);
  71         netlink_rcv_cb(skb);
  72         mutex_unlock(&netlink_mutex);
  73 }
  74
  75 struct sock *netlink_init(int unit,
  76                           void (*cb)(struct net_device *dev, u16 type,
  77                                      void *msg, int len))
  78 {
  79         struct sock *sock;
  80         struct netlink_kernel_cfg cfg = {
  81                 .input  = netlink_rcv,
  82         };
  83
  84         sock = netlink_kernel_create(&init_net, unit, &cfg);
  85
  86         if (sock)
  87                 rcv_cb = cb;
  88
  89         return sock;
  90 }
  91
  92 int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
  93 {
  94         static u32 seq;
  95         struct sk_buff *skb = NULL;
  96         struct nlmsghdr *nlh;
  97         int ret = 0;
  98
  99         if (group > ND_MAX_GROUP)
 100                 return -EINVAL;
 101
 102         if (!netlink_has_listeners(sock, group + 1))
 103                 return -ESRCH;
 104
 105         skb = alloc_skb(NLMSG_SPACE(len), GFP_ATOMIC);
 106         if (!skb)
 107                 return -ENOMEM;
 108
 109         seq++;
 110
 111         nlh = nlmsg_put(skb, 0, seq, type, len, 0);
 112         memcpy(NLMSG_DATA(nlh), msg, len);
 113         NETLINK_CB(skb).portid = 0;
 114         NETLINK_CB(skb).dst_group = 0;
 115
 116         ret = netlink_broadcast(sock, skb, 0, group + 1, GFP_ATOMIC);
 117         if (!ret)
 118                 return len;
 119
 120         if (ret != -ESRCH)
 121                 pr_err("nl broadcast g=%d, t=%d, l=%d, r=%d\n",
 122                        group, type, len, ret);
 123         else if (netlink_has_listeners(sock, group + 1))
 124                 return -EAGAIN;
 125
 126         return ret;
 127 }