netlink_test.c

#include <net/sock.h>
#include <linux/module.h>
#include <linux/netlink.h>
#include <linux/skbuff.h>
#include <linux/kthread.h>  // for threads

#include "daemon_messages.h"

#define MYPROTO NETLINK_USERSOCK
#define MYGRP 17



struct sock *nl_sock = NULL;

static struct task_struct *g_thread = 0;
static char thread_name[] = {"test thread"};


static void netlink_test_recv_msg(struct sk_buff *skb)
{
    struct sk_buff *skb_out;
    struct nlmsghdr *nlh;
    int msg_size;
    char *msg;
    int pid;
    int res;

    nlh = (struct nlmsghdr *)skb->data;
    pid = nlh->nlmsg_pid; /* pid of sending process */
    //msg = (char *)nlmsg_data(nlh);
    //msg_size = strlen(msg);

    dolphin_mc_alloc_resp * resp = (dolphin_mc_alloc_resp *) nlmsg_data(nlh);

    printk(KERN_INFO "netlink_test: Received %d bytes from pid %d:\n", nlmsg_len(nlh), pid);
    printk(KERN_INFO "netlink_test: msg_type %u, status %u, num_addrs %u\n", resp->header.msg_id, resp->status, resp->num_addrs);
    for(int i=0; i < resp->num_addrs; i+=2) 
    {
        printk(KERN_INFO "netlink_test: \t read addr: %p\n", resp->addrs[i]);
        printk(KERN_INFO "netlink_test: \t write addr %p\n", resp->addrs[i+1]);
    }


    /*

    // create reply
    skb_out = nlmsg_new(msg_size, 0);
    if (!skb_out) {
        printk(KERN_ERR "netlink_test: Failed to allocate new skb\n");
        return;
    }

    // put received message into reply
    nlh = nlmsg_put(skb_out, 0, 0, NLMSG_DONE, msg_size, 0);
    //NETLINK_CB(skb_out).dst_group = MYGRP; / in multicast group 
    strncpy(nlmsg_data(nlh), msg, msg_size);

    printk(KERN_INFO "netlink_test: Send %s\n", msg);
    res = nlmsg_multicast(nl_sock, skb_out, 0, MYGRP, GFP_KERNEL);

    if (res < 0)
        printk(KERN_INFO "netlink_test: Error while sending skb to user\n");
        */
}



int multicast_thread_fn( void * pv )
{

    struct sk_buff *skb_out;
    struct nlmsghdr *nlh;
    int res;
    dolphin_mc_alloc_req * alloc_req;

    unsigned num_segments = 2;
    unsigned total_payload_sz = sizeof( dolphin_mc_alloc_req ) + sizeof(uint32_t) * num_segments;

    //Send one request
    skb_out = nlmsg_new( total_payload_sz, 0);
    if (!skb_out) {
        printk(KERN_ERR "netlink_test: Failed to allocate new skb\n");
        while(!kthread_should_stop())
        {
            msleep(1000);
        }
        return 0;
    }

    //Allocate and send message
    nlh = nlmsg_put(skb_out, 0, 0, NLMSG_DONE, total_payload_sz, 0);
    //strncpy(nlmsg_data(nlh), msg, msg_size);



    alloc_req = (dolphin_mc_alloc_req*) nlmsg_data(nlh);
    alloc_req->header.msg_id = DOLPHIN_DAEMON_ALLOC_REQ;
    alloc_req->num_segments = num_segments;
    for(int i =0 ; i < num_segments; ++i)
    {
        alloc_req->segment_ids[i] = i;
    } 

    res = nlmsg_multicast(nl_sock, skb_out, 0, MYGRP, GFP_KERNEL);
    skb_out = NULL;


    while(!kthread_should_stop()) 
    {
        msleep(1000);
    }
    return 0;
}

/*
void real_time_loop( void )
{
    struct sk_buff *skb_out;
    struct nlmsghdr *nlh;
    int res;
    const char * msg = "Hello from an isolated core.";
    size_t msg_size = 128;

    while(atomic_read(&g_atom_should_exit) == 0)
    {
        skb_out = nlmsg_new(1024, 0);
        if (!skb_out) {
            printk(KERN_ERR "netlink_test: Failed to allocate new skb\n");
            msleep(1000);
            continue;
        }

        //Allocate and send message
        nlh = nlmsg_put(skb_out, 0, 0, NLMSG_DONE, msg_size, 0);
        strncpy(nlmsg_data(nlh), msg, msg_size);

        res = nlmsg_multicast(nl_sock, skb_out, 0, MYGRP, GFP_KERNEL);
        skb_out = NULL;

        mdelay(250);
    }

    atomic_set(&g_atom_has_exited, 1);
    return;
}*/



static int __init netlink_test_init(void)
{
    printk(KERN_INFO "netlink_test: Init module\n");

    struct netlink_kernel_cfg cfg = {
        .input = netlink_test_recv_msg,
    //    .groups = MYGRP
    };

    nl_sock = netlink_kernel_create(&init_net, MYPROTO, &cfg);
    if (!nl_sock) {
        printk(KERN_ALERT "netlink_test: Error creating socket.\n");
        return -10;
    }

    g_thread = kthread_run(multicast_thread_fn, NULL, thread_name);
    if( !g_thread )
    {
        printk(KERN_ERR  " - Could not create the reader kthread.\n");
        return -5;
    }


    return 0;
}

static void __exit netlink_test_exit(void)
{
    printk(KERN_INFO "netlink_test: Exit module\n");

    if(g_thread)
        kthread_stop(g_thread);

    netlink_kernel_release(nl_sock);
}

module_init(netlink_test_init);
module_exit(netlink_test_exit);

MODULE_LICENSE("Dual MIT/GPL");