icwmp/kcwmp.c

/*
 * Copyright (C) 2019 iopsys Software Solutions AB. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * version 2 as published by the Free Software Foundation.
 *
 */

#include <linux/module.h>
#include <net/sock.h>
#include <linux/netlink.h>
#include <linux/skbuff.h>
#include <linux/mutex.h>
#include <linux/wait.h>
#include <stdarg.h>
#include <linux/slab.h>
#include "cwmp_kernel.h"

typedef struct kernel_cwmp_input {
	char **argv;
	int argc;
	int handler;
	struct mutex mutex;
	wait_queue_head_t thresholdq;
} KERNEL_CWMP_INPUT;

static struct sock *nl_sk = NULL;
static struct kernel_cwmp_input kernel_cwmp_input;

MODULE_LICENSE("IOPSYS");

static void kernel_api_cwmp_value_change_listener(struct sk_buff *skb)
{
	struct nlmsghdr *nlh;
	int pid;
	char *msg;
	char *recv;
	int i;

	nlh = (struct nlmsghdr *)skb->data;
	recv = (char *)nlmsg_data(nlh);

	if (strcmp(recv, NETLINK_CWMP_ID) != 0) {
		return;
	}
	pid = nlh->nlmsg_pid; /*pid of sending process */

	while (kernel_cwmp_input.argc == 0) {
		if (wait_event_interruptible(kernel_cwmp_input.thresholdq, kernel_cwmp_input.handler)) {
			return;
		}
	}
	kernel_cwmp_input.handler = 0;

	mutex_lock(&(kernel_cwmp_input.mutex));

	for (i = 0; i <= kernel_cwmp_input.argc; i++) {
		struct sk_buff *skb_out;
		int msg_size, res;

		if (i < kernel_cwmp_input.argc) {
			msg = kernel_cwmp_input.argv[i];
		} else {
			msg = NETLINK_END_DATA;
		}
		msg_size = strlen(msg);
		skb_out = nlmsg_new(msg_size, 0);
		if (!skb_out) {
			printk(KERN_ERR "Failed to allocate new skb\n");
			return;
		}

		nlh = nlmsg_put(skb_out, 0, 0, NLMSG_DONE, msg_size, 0);
		NETLINK_CB(skb_out).dst_group = 0; /* not in mcast group */
		strncpy(nlmsg_data(nlh), msg, msg_size);
		res = nlmsg_unicast(nl_sk, skb_out, pid);

		if (i < kernel_cwmp_input.argc) {
			kfree(kernel_cwmp_input.argv[i]);
		}
	}
	if (kernel_cwmp_input.argv != NULL) {
		kfree(kernel_cwmp_input.argv);
		kernel_cwmp_input.argv = NULL;
	}

	kernel_cwmp_input.argc = 0;
	mutex_unlock(&(kernel_cwmp_input.mutex));
}

static int /*__init*/ kernel_api_cwmp_value_change_init(void)
{
	printk(KERN_INFO "Entering kernel cwmp module: %s\n", __FUNCTION__);
	memset(&kernel_cwmp_input, 0, sizeof(struct kernel_cwmp_input));
	init_waitqueue_head(&kernel_cwmp_input.thresholdq);
	mutex_init(&kernel_cwmp_input.mutex);
	nl_sk = netlink_kernel_create(&init_net, NETLINK_USER, 0, kernel_api_cwmp_value_change_listener, NULL, THIS_MODULE);
	if (!nl_sk) {
		printk(KERN_ALERT "Error creating socket.\n");
		return -10;
	}
	return 0;
}

static void /*__exit*/ kernel_api_cwmp_value_change_exit(void)
{
	printk(KERN_INFO "Exiting kernel cwmp module: %s\n", __FUNCTION__);
	netlink_kernel_release(nl_sk);
}

int kernel_api_cwmp_value_change_call(int count, ...)
{
	int i;
	va_list args;
	char *s;

	if (kernel_cwmp_input.argc > 0) {
		kernel_cwmp_input.handler = 1;
		wake_up_interruptible(&(kernel_cwmp_input.thresholdq));
		return 1;
	}

	mutex_lock(&(kernel_cwmp_input.mutex));
	kernel_cwmp_input.argv = kmalloc(count * sizeof(char *), GFP_KERNEL);

	if (kernel_cwmp_input.argv == NULL) {
		goto kernel_api_cwmp_error;
	}

	va_start(args, count);
	for (i = 0; i < count; i++) {
		s = (char *)va_arg(args, char *);
		if (s == NULL) {
			s = NETLINK_NULL;
		}
		kernel_cwmp_input.argv[i] = kmalloc(strlen(s), GFP_KERNEL);
		if (kernel_cwmp_input.argv[i] == NULL) {
			goto kernel_api_cwmp_error;
		}
		strncpy(kernel_cwmp_input.argv[i], s, sizeof(kernel_cwmp_input.argv[i]));
		kernel_cwmp_input.argc++;
	}
	va_end(args);
	mutex_unlock(&(kernel_cwmp_input.mutex));
	kernel_cwmp_input.handler = 1;
	wake_up_interruptible(&(kernel_cwmp_input.thresholdq));
	return 1;

kernel_api_cwmp_error:

	if (kernel_cwmp_input.argv != NULL) {
		for (i = 0; i < kernel_cwmp_input.argc; i++) {
			if (kernel_cwmp_input.argv[i] != NULL) {
				kfree(kernel_cwmp_input.argv[i]);
			}
		}
		kfree(kernel_cwmp_input.argv);
		kernel_cwmp_input.argv = NULL;
	}
	kernel_cwmp_input.argc = 0;
	mutex_unlock(&(kernel_cwmp_input.mutex));
	return 1;
}
EXPORT_SYMBOL(kernel_api_cwmp_value_change_call);

module_init(kernel_api_cwmp_value_change_init);
module_exit(kernel_api_cwmp_value_change_exit);