arch/arm/plat-omap/sti/sti-netlink.c

   1 /*
   2  * OMAP STI/XTI communications interface via netlink socket.
   3  *
   4  * Copyright (C) 2004, 2005, 2006 Nokia Corporation
   5  * Written by: Paul Mundt <paul.mundt@nokia.com>
   6  *
   7  * This file is subject to the terms and conditions of the GNU General Public
   8  * License.  See the file "COPYING" in the main directory of this archive
   9  * for more details.
  10  */
  11 #include <linux/init.h>
  12 #include <linux/kernel.h>
  13 #include <linux/netlink.h>
  14 #include <linux/socket.h>
  15 #include <linux/skbuff.h>
  16 #include <linux/mutex.h>
  17 #include <net/sock.h>
  18 #include <asm/arch/sti.h>
  19
  20 static struct sock *sti_sock;
  21 static DEFINE_MUTEX(sti_netlink_mutex);
  22
  23 enum {
  24         STI_READ,
  25         STI_WRITE,
  26 };
  27
  28 static int sti_netlink_read(int pid, int seq, void *payload, int size)
  29 {
  30         struct sk_buff *skb;
  31         struct nlmsghdr *nlh;
  32         int ret, len = NLMSG_SPACE(size);
  33         unsigned char *tail;
  34
  35         skb = alloc_skb(len, GFP_KERNEL);
  36         if (!skb)
  37                 return -ENOMEM;
  38
  39         tail = skb->tail;
  40         nlh = NLMSG_PUT(skb, pid, seq, STI_READ,
  41                         len - (sizeof(struct nlmsghdr)));
  42         nlh->nlmsg_flags = 0;
  43         memcpy(NLMSG_DATA(nlh), payload, size);
  44         nlh->nlmsg_len = skb->tail - tail;
  45
  46         ret = netlink_unicast(sti_sock, skb, pid, MSG_DONTWAIT);
  47         if (ret > 0)
  48                 ret = 0;
  49
  50         return ret;
  51
  52 nlmsg_failure:
  53         if (skb)
  54                 kfree_skb(skb);
  55
  56         return -EINVAL;
  57 }
  58
  59 /*
  60  * We abuse nlmsg_type and nlmsg_flags for our purposes.
  61  *
  62  * The ID is encoded into the upper 8 bits of the nlmsg_type, while the
  63  * channel number is encoded into the upper 8 bits of the nlmsg_flags.
  64  */
  65 static int sti_netlink_receive_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
  66 {
  67         void *data;
  68         u8 chan, id;
  69         int size, ret = 0, len = 0;
  70
  71         data    = NLMSG_DATA(nlh);
  72         chan    = (nlh->nlmsg_flags >> 8) & 0xff;
  73         id      = (nlh->nlmsg_type  >> 8) & 0xff;
  74         size    = (int)(nlh->nlmsg_len - ((char *)data - (char *)nlh));
  75
  76         switch (nlh->nlmsg_type & 0xff) {
  77         case STI_WRITE:
  78                 sti_channel_write_trace(size, id, data, chan);
  79                 break;
  80         case STI_READ:
  81                 data = kmalloc(size, GFP_KERNEL);
  82                 if (!data)
  83                         return -ENOMEM;
  84                 memset(data, 0, size);
  85
  86                 len = sti_read_packet(data, size);
  87                 ret = sti_netlink_read(NETLINK_CB(skb).pid, nlh->nlmsg_seq,
  88                                        data, len);
  89                 kfree(data);
  90                 break;
  91         default:
  92                 return -ENOTTY;
  93         }
  94
  95         return ret;
  96 }
  97
  98 static int sti_netlink_receive_skb(struct sk_buff *skb)
  99 {
 100         while (skb->len >= NLMSG_SPACE(0)) {
 101                 struct nlmsghdr *nlh;
 102                 u32 rlen;
 103                 int ret;
 104
 105                 nlh = (struct nlmsghdr *)skb->data;
 106                 if (nlh->nlmsg_len < sizeof(struct nlmsghdr) ||
 107                     skb->len < nlh->nlmsg_len)
 108                         break;
 109
 110                 rlen = NLMSG_ALIGN(nlh->nlmsg_len);
 111                 if (rlen > skb->len)
 112                         rlen = skb->len;
 113
 114                 ret = sti_netlink_receive_msg(skb, nlh);
 115                 if (ret)
 116                         netlink_ack(skb, nlh, -ret);
 117                 else if (nlh->nlmsg_flags & NLM_F_ACK)
 118                         netlink_ack(skb, nlh, 0);
 119
 120                 skb_pull(skb, rlen);
 121         }
 122
 123         return 0;
 124 }
 125
 126 static void sti_netlink_receive(struct sk_buff *skb)
 127 {
 128         if (!mutex_trylock(&sti_netlink_mutex))
 129                 return;
 130
 131         sti_netlink_receive_skb(skb);
 132         mutex_unlock(&sti_netlink_mutex);
 133 }
 134
 135 static int __init sti_netlink_init(void)
 136 {
 137         sti_sock = netlink_kernel_create(&init_net, NETLINK_USERSOCK, 0,
 138                                          sti_netlink_receive, NULL,
 139                                          THIS_MODULE);
 140         if (!sti_sock) {
 141                 printk(KERN_ERR "STI: Failed to create netlink socket\n");
 142                 return -ENODEV;
 143         }
 144
 145         return 0;
 146 }
 147
 148 module_init(sti_netlink_init);
 149
 150 MODULE_AUTHOR("Paul Mundt");
 151 MODULE_LICENSE("GPL");
 152 MODULE_DESCRIPTION("STI netlink-driven communications interface");