/** @file knetlink.c
 *
 * @author marco corvi <marco_corvi@geocities.com>
 * @date feb 2005
 * 
 * @brief netlink kernel module
 *
 * inspired and heavily taken from the article
 * K. Kaichuan He, "Why and how to use netlink socket"
 * Linux Journal, Feb 2005
 *
 */
#ifndef __KERNEL__
#  define __KERNEL__
#endif

#define __NO_VERSION__ /* don't define kernel_verion in module.h */
#include <linux/init.h>
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/sched.h>

#include <linux/kthread.h>
#include <linux/net.h>
#include <net/sock.h>
#include <linux/socket.h>
#include <linux/netlink.h>
#include <linux/skbuff.h>

char kernel_version [] = UTS_RELEASE;

#define KNETLINK_UNIT 17

static struct sock * knetlink_sk = NULL;

#define KNETLINK_USE_KTHREAD 1

#ifdef KNETLINK_USE_KTHREAD
  struct task_struct * knetlink_task = NULL;
  int knetlink_thread_should_stop;
  extern struct task_struct *kthread_create(
    int (*threadfn)(void *data), void *data, const char namefmt[], ...);
  extern int kthread_stop(struct task_struct *k);
#endif

/** process a netlink message
 * @param skb socket buffer containing the netlink message
 *
 * The netlink message is in skb-> data
 * This function does some printout, modifies the bytes of the payload,
 * and send a reply message back to the sender user process.
 */
void knetlink_process( struct sk_buff * skb )
{
  int k;
  struct nlmsghdr * nlh = NULL;
  u8 * payload = NULL;
  int   payload_size;
  int   length;
  int   seq;
  pid_t pid;
  struct sk_buff * rskb; 

  /* process netlink message pointed by skb->data */
  nlh = (struct nlmsghdr *)skb->data;
  pid = nlh->nlmsg_pid;
  length = nlh->nlmsg_len;
  seq = nlh->nlmsg_seq;
  printk("knetlink_process: nlmsg len %d type %d pid %d seq %d\n",
    length, nlh->nlmsg_type, pid, seq );
  /* process the paylad */
  payload_size = nlh->nlmsg_len - NLMSG_LENGTH(0);
  if ( payload_size > 0 ) {
    payload = NLMSG_DATA( nlh );
    printk("knetlink_process: process payload ");
    for (k=0; k<10; k++) printk("%02x ", payload[k]);
    printk("...\n");
    for ( k=0; k<payload_size; ++k)
      payload[k] = 0xff - payload[k];
    /* ... */
  }
  // reply
  rskb = alloc_skb( nlh->nlmsg_len, GFP_KERNEL );
  if ( rskb ) {
    memcpy( rskb->data, skb->data, length );
    skb_put( rskb, length );
    kfree_skb( skb );
  } else {
    printk("knetlink_process: replies with the same socket_buffer\n");
    rskb = skb;
  }
  nlh = (struct nlmsghdr *) rskb->data;
  nlh->nlmsg_len   = length;
  nlh->nlmsg_pid   = 0; /* from kernel */
  nlh->nlmsg_flags = 0;
  nlh->nlmsg_type  = 2;
  nlh->nlmsg_seq   = seq+1;
  NETLINK_CB( rskb ).groups = 0;
  NETLINK_CB( rskb ).pid    = 0;
  NETLINK_CB( rskb ).dst_groups = 0;
  NETLINK_CB( rskb ).dst_pid    = pid;
  payload = NLMSG_DATA( nlh );

  printk("knetlink_process: reply nlmsg len %d type %d pid %d\n",
    nlh->nlmsg_len, nlh->nlmsg_type, nlh->nlmsg_pid );
  printk("knetlink_process: payload ");
  for (k=0; k<10; k++) printk("%02x ", payload[k]);
  printk("...\n");

  netlink_unicast( knetlink_sk, rskb, pid, MSG_DONTWAIT ); 

}

/** kernel thread function
 * @param data sock of the module (namely knetlink_sk)
 * @return 0
 * 
 * This function waits for incoming socket buffers
 * and calls the knetlink_process on each
 */
#ifdef KNETLINK_USE_KTHREAD
int knetlink_thread( void * data )
{
  struct sock * sk = (struct sock *)data;
  struct sk_buff * skb;
  DECLARE_WAITQUEUE( queue, current );
  printk("knetlink_thread: started sock %p\n", (void *)sk );

  while ( sk ) {
    // the following is just
    //   interruptible_sleep_on( sk->sk_sleep );
    // to get the socket buffer should do
    //   skb = skb_recv_datagram( sk, 0, 0, &err );
    skb = NULL;
    add_wait_queue( sk->sk_sleep, &queue );
    for (;;) {
      set_current_state( TASK_INTERRUPTIBLE );
      skb = skb_dequeue( &sk->sk_receive_queue );
      printk("knetlink_thread: skb_dequeue skb %p\n", (void *)skb);
      if ( skb != NULL )
        break;
      if ( ! signal_pending( current ) ) {
        printk("knetlink_thread: schedule\n");
        schedule();
        if ( knetlink_thread_should_stop ) goto done;
        continue;
      }
      printk("knetlink_thread: signal pending!\n");
    }
    printk("knetlink_thread: continue skb %p\n", (void *)skb);
    current->state = TASK_RUNNING;
    remove_wait_queue( sk->sk_sleep, &queue );
    if ( skb != NULL ) {
      printk("knetlink_thread: skb %p \n", (void *)skb );
      knetlink_process( skb );
    }
  }
done:
  return 0;
  }
#endif

/** input function
 * @param sk sock (namely knetlink_sk)
 * @param len length of the data on the sock
 *
 * Either it calls the knetlink_process (no-thread model)
 * or it wakes the knetlink thread up.
 */ 
void knetlink_input( struct sock * sk, int len )
{
  printk("knetlink_input: sock %p, len %d\n", (void*)sk, len);
  if ( knetlink_sk != sk ) {
    printk("knetlink_input: wrong sock %p instead of %p\n",
      (void *)sk, (void *)knetlink_sk );
    return;
  }
  #ifdef KNETLINK_USE_KTHREAD
    wake_up_interruptible( knetlink_sk->sk_sleep );
  #else
  {
    struct sk_buff * skb;
    while ( (skb = skb_dequeue( &sk->sk_receive_queue ) ) != NULL ) {
      knetlink_process( skb );
    }
  }
  #endif
}

/** module init
 * @return 0 on success
 *
 * It creates the netlink, and (in the thread model) the
 * knetlink thread
 */
int knetlink_init( void )
{
  if ( knetlink_sk != NULL ) {
    printk("knetlink_init: sock already present\n");
    return 1;
  }

  knetlink_sk = netlink_kernel_create( KNETLINK_UNIT, knetlink_input);
  if ( knetlink_sk == NULL ) {
    printk("knetlink_init: sock fail\n");
    return 1;
  }

  #ifdef KNETLINK_USE_KTHREAD
    knetlink_thread_should_stop = 0;

    knetlink_task = kthread_create(
                      knetlink_thread, (void *)knetlink_sk, "knl-thread" );
    if ( knetlink_task == NULL ) {
      printk("knetlink_init: failed to create kernel thread\n");
      sock_release( knetlink_sk->sk_socket );
      return 1;
    } else {
      if ( wake_up_process( knetlink_task ) == 0 ) {
        printk("knetlink_init: thread %p already awake\n",
          (void*)knetlink_task);
      } else {
        printk("knetlink_init: thread %p waken up\n",
          (void*)knetlink_task);
      }
    }
  #endif

  printk("knetlink_init: sock %p\n", (void*)knetlink_sk );
  return 0;
}

/** module exit
 *
 * In the thread model it stops the thread,
 * then it releases the knetlink sock.
 */
void knetlink_exit( void )
{
  #ifdef KNETLINK_USE_KTHREAD
    if ( knetlink_task ) {
      knetlink_thread_should_stop = 1;
      printk("knetlink_exit: stop thread\n");
      kthread_stop( knetlink_task );
    }
  #endif

  if ( knetlink_sk != NULL ) {
    printk("knetlink_exit: release sock %p\n", (void*)knetlink_sk);
    sock_release( knetlink_sk->sk_socket );
  } else {
    printk("knetlink_exit: warning sock is NULL\n");
  }

}

module_init( knetlink_init );
module_exit( knetlink_exit );

MODULE_LICENSE("GPL");
MODULE_AUTHOR("marco corvi");
MODULE_DESCRIPTION("Netlink example");