bcm68620_release/release/host_customized/transport/nl_socket/bcmolt_tr_nl_driver.c

/*
<:copyright-BRCM:2016:DUAL/GPL:standard

   Broadcom Proprietary and Confidential.(c) 2016 Broadcom
   All Rights Reserved

Unless you and Broadcom execute a separate written software license
agreement governing use of this software, this software is licensed
to you under the terms of the GNU General Public License version 2
(the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php,
with the following added to such license:

   As a special exception, the copyright holders of this software give
   you permission to link this software with independent modules, and
   to copy and distribute the resulting executable under terms of your
   choice, provided that you also meet, for each linked independent
   module, the terms and conditions of the license of that module.
   An independent module is a module which is not derived from this
   software.  The special exception does not apply to any modifications
   of the software.

Not withstanding the above, under no circumstances may you combine
this software in any way with any other Broadcom software provided
under a license other than the GPL, without Broadcom's express prior
written consent.

:>
 */

/*
 * nl_transport.c
 *
 * NL-socket based transport. Kernel space module
 *
 */
#include <linux/module.h>       /* Needed by all modules */
#include <linux/kernel.h>       /* Needed for KERN_INFO */
#include <net/netlink.h>
#include <net/sock.h>
#include <linux/netdevice.h>
#include <linux/version.h>

#include <bcmolt_tr_mux.h>
#include <bcmolt_tr_nl_driver.h>
#include "bcmolt_tr_nl.h"

#define BCMTR_MEM_PACKETS       10000
#define BCMTR_MEM_OVERHEAD      SKB_TRUESIZE(1024)
#define BCMTR_MIN_RCVBUF_SIZE   (BCMTR_MEM_PACKETS * BCMTR_MEM_OVERHEAD)

struct sock *bcmtr_nl_sock = NULL;

/* Netlink operations. This driver overrides a couple */
static struct proto_ops bcmtr_nl_ops;
static int (*netlink_release_org)(struct socket *sock);

struct bcmtr_nl
{
    bcmtrmux_channel channel[BCMTR_MAX_OLTS];
    int pid;
    struct sock *sk;
};

static int bcmtr_netlink_release(struct socket *sock)
{
    struct sock *sk = sock->sk;
    struct bcmtr_nl *nlb = sk->sk_user_data;
    if (nlb)
    {
        int i;
        for (i = 0; i < BCMTR_MAX_OLTS; i++)
        {
            if (nlb->channel[i])
                bcmtrmux_channel_unregister(i, nlb->channel[i]);
        }
        kfree(nlb);
        sk->sk_user_data = NULL;
    }
    return netlink_release_org(sock);
}

/** Receive message handler */
static  void _bcmtr_nl_recv(bcmolt_devid device, bcmos_buf *buf, bcmtrmux_channel channel, void *data)
{
    uint32_t data_length = bcmos_buf_length(buf);
    uint32_t device_u32 = (uint32_t)device;
    struct bcmtr_nl *nlb = (struct bcmtr_nl *)data;
    uint8_t *p_device;
    int r;

    NETLINK_CB(&(buf->skb)).dst_group = 0; /* not in mcast group */

    /* Now a tricky part. nlmsg_put() assumes that skb is empty and extends it
     * by total msg size. We don't want it. Reserve room for nlmsg header and then
     * set size to 0 and let nlmsg_put() restore it
     */
    if (unlikely(skb_headroom(&buf->skb) < NLMSG_HDRLEN + sizeof(uint32_t)))
    {
        printk(KERN_INFO "%s: can't prepend NL header\n", __FUNCTION__);
        bcmos_free(buf);
        return;
    }
    __skb_push(&buf->skb, NLMSG_HDRLEN + sizeof(uint32_t));
    /* Store device immediately after the NL header */
    p_device = buf->skb.data + NLMSG_HDRLEN;
    memcpy(p_device, &device_u32, sizeof(uint32_t));
    __skb_trim(&buf->skb, 0);
    nlmsg_put(&buf->skb, 0, 0, NLMSG_DONE, data_length + sizeof(uint32_t), 0);

    r = nlmsg_unicast(nlb->sk, &buf->skb, nlb->pid);
    if (r)
    {
        printk(KERN_INFO "%s: nlmsg_unicast() --> %d len=%u rm_alloc=%d recvbuf=%d\n",
            __FUNCTION__, r, data_length, atomic_read(&nlb->sk->sk_rmem_alloc), nlb->sk->sk_rcvbuf);
    }
}

void bcmtr_nl_input(struct sk_buff *skb)
{
    struct nlmsghdr *nlh;
    struct bcmtr_nl *nlb;
    uint32_t device;
    bcmos_errno rc;

    nlh = nlmsg_hdr(skb);

    if(NLMSG_OK(nlh, skb->len))
    {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0)
        struct sock *sk = NETLINK_CB(skb).sk;
#else
        struct sock *sk = NETLINK_CB(skb).ssk;
#endif

        /* channel is stored in user_data. If it is 0 - that means that channel
         * is not allocated yet. Do it here
         */
        if (!sk->sk_user_data)
        {
            nlb = kmalloc(sizeof(*nlb), GFP_KERNEL);
            if (!nlb)
            {
                printk(KERN_INFO "%s: can't allocate NLB\n", __FUNCTION__);
                return;
            }
            memset(nlb, 0, sizeof(*nlb));
            nlb->pid = nlh->nlmsg_pid;
            nlb->sk = sk;
            sk->sk_user_data = nlb;

            /* Override release callback in order to cleanup */
            bcmtr_nl_ops = *sk->sk_socket->ops;
            netlink_release_org = bcmtr_nl_ops.release;
            bcmtr_nl_ops.release = bcmtr_netlink_release;
            sk->sk_socket->ops = &bcmtr_nl_ops;

            /* Override sk_rcvbuf size. Set min */
            if (sk->sk_rcvbuf < BCMTR_MIN_RCVBUF_SIZE)
            {
                sk->sk_rcvbuf = BCMTR_MIN_RCVBUF_SIZE;
            }
        }
        else
        {
            nlb = (struct bcmtr_nl *)sk->sk_user_data;
        }

        /* Device id follows NLMSG header */
        device = skb->data[NLMSG_HDRLEN];
        if (device >= BCMTR_MAX_OLTS)
        {
            printk(KERN_INFO "%s: device %u is insane\n", __FUNCTION__, device);
            return;
        }

        if (!nlb->channel[device])
        {
            bcmtrmux_channel channel = BCMTRMUX_CHANNEL_AUTO_ASSIGN;

            rc = bcmtrmux_channel_register(device, &channel, _bcmtr_nl_recv, nlb);
            if (rc)
            {
                printk(KERN_INFO "%s: can't register channel for device %u. rc=%d\n", __FUNCTION__, device, (int)rc);
                return;
            }
            nlb->channel[device] = channel;
        }


        /* Cut NL header + device */
        skb_pull(skb, NLMSG_HDRLEN + sizeof(uint32_t));

        /* Clone skb because caller will release the original one */
        skb = skb_clone(skb, 0);
        if (!skb)
        {

            printk(KERN_ERR "Failed to allocate new skb\n");
            return;
        }

        /* Forward to the mux */
        bcmtrmux_rx_from_host(device, nlb->channel[device], (bcmos_buf *)skb);
    }
    else
    {
        printk(KERN_INFO "%s: invalid message received frpm pid %d\n", __FUNCTION__, nlh->nlmsg_pid);
    }
}

static struct netlink_kernel_cfg bcmtr_nl_cfg = {
    .input = bcmtr_nl_input,
};

bcmos_errno bcmtr_nl_init(void)
{
    printk(KERN_INFO "Maple netlink-based raw driver transport driver\n");
    bcmtr_nl_sock = netlink_kernel_create(&init_net, BCMTR_NL_FAMILY, &bcmtr_nl_cfg);
    if(!bcmtr_nl_sock)
        return BCM_ERR_NOMEM;
    return BCM_ERR_OK;
}

void bcmtr_nl_exit(void)
{
    printk("Maple netlink-based raw driver terminated\n");
    if (bcmtr_nl_sock)
    {
        netlink_kernel_release(bcmtr_nl_sock);
        bcmtr_nl_sock = NULL;
    }
}