Kernel connector - userspace <-> kernelspace "linker".

[Evgeniy Polyakov <johnpol@2ka.mipt.ru>]

[-- Attachment #1.1: Type: text/plain, Size: 1118 bytes --]

Hmm, do not know how to describe... 
Kind of mega-picture can be found at 
http://tservice.net.ru/~s0mbre/?section=gallery&item=connector_design

This driver adds possibility to connect anything with anything using
netlink based network.
One must register callback and identificator. When driver receives
special netlink message with appropriate identificator, appropriate
callback will be called.
I think that the code better explains what I'm trying to say.

cn_queue.[ch] - main queue processing routings.
connector.[ch] - interface to the external modules.
ucon.c - userspace daemon. It is broken a bit, but the main idea is 
	very clear.
cn_test.c - module to test new connector.
Makefile - it will link all above cruft.

Origianlly this was written for SuperIO and w1 subsystems to connect 
them to userspace, but actually it can be used outside those projects.
Greg KH recommended to send it to linux-kernl@ mail list, but I personally 
no not like that too "floodfil" list.

Please review and comment.

-- 
	Evgeniy Polyakov

Crash is better than data corruption. -- Art Grabowski

[-- Attachment #1.2: cn_queue.c --]
[-- Type: text/x-csrc, Size: 5361 bytes --]

/*
 * 	cn_queue.c
 * 
 * 2004 Copyright (c) Evgeniy Polyakov <johnpol@2ka.mipt.ru>
 * 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/list.h>
#include <linux/workqueue.h>
#include <linux/spinlock.h>
#include <linux/slab.h>
#include <linux/skbuff.h>
#include <linux/suspend.h>

#include "cn_queue.h"

static void cn_queue_wrapper(void *data)
{
	struct cn_callback_entry *cbq = (struct cn_callback_entry *)data;

	atomic_inc(&cbq->cb->refcnt);
	cbq->cb->callback(cbq->cb->priv);
	atomic_dec(&cbq->cb->refcnt);

	cbq->destruct_data(cbq->ddata);
}

static struct cn_callback_entry *cn_queue_alloc_callback_entry(struct
							       cn_callback *cb)
{
	struct cn_callback_entry *cbq;

	cbq = kmalloc(sizeof(*cbq), GFP_KERNEL);
	if (!cbq) {
		printk(KERN_ERR "Failed to create new callback queue.\n");
		return NULL;
	}

	memset(cbq, 0, sizeof(*cbq));

	cbq->cb = cb;

	INIT_WORK(&cbq->work, &cn_queue_wrapper, cbq);

	return cbq;
}

static void cn_queue_free_callback(struct cn_callback_entry *cbq)
{
	cancel_delayed_work(&cbq->work);

	while (atomic_read(&cbq->cb->refcnt)) {
		printk(KERN_INFO "Waiting %s to became free: refcnt=%d.\n",
		       cbq->pdev->name, atomic_read(&cbq->cb->refcnt));
		set_current_state(TASK_INTERRUPTIBLE);
		schedule_timeout(HZ);

		if (current->flags & PF_FREEZE)
			refrigerator(PF_FREEZE);

		if (signal_pending(current))
			flush_signals(current);
	}

	kfree(cbq);
}

int cn_cb_equal(struct cb_id *i1, struct cb_id *i2)
{
	return ((i1->idx == i2->idx) && (i1->val == i2->val));
}

int cn_queue_add_callback(struct cn_queue_dev *dev, struct cn_callback *cb)
{
	struct cn_callback_entry *cbq, *n, *__cbq;
	int found = 0;

	cbq = cn_queue_alloc_callback_entry(cb);
	if (!cbq)
		return -ENOMEM;

	atomic_inc(&dev->refcnt);
	cbq->pdev = dev;

	spin_lock(&dev->queue_lock);
	list_for_each_entry_safe(__cbq, n, &dev->queue_list, callback_entry) {
		if (cn_cb_equal(&__cbq->cb->id, &cb->id)) {
			found = 1;
			break;
		}
	}
	if (!found) {
		atomic_set(&cbq->cb->refcnt, 1);
		list_add_tail(&cbq->callback_entry, &dev->queue_list);
	}
	spin_unlock(&dev->queue_lock);

	if (found) {
		atomic_dec(&dev->refcnt);
		atomic_set(&cbq->cb->refcnt, 0);
		cn_queue_free_callback(cbq);
		return -EINVAL;
	}

	cbq->nls = dev->nls;
	cbq->seq = 0;
	cbq->group = ++dev->netlink_groups;

	return 0;
}

void cn_queue_del_callback(struct cn_queue_dev *dev, struct cn_callback *cb)
{
	struct cn_callback_entry *cbq = NULL, *n;
	int found = 0;

	spin_lock(&dev->queue_lock);
	list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry) {
		if (cn_cb_equal(&cbq->cb->id, &cb->id)) {
			list_del(&cbq->callback_entry);
			found = 1;
			break;
		}
	}
	spin_unlock(&dev->queue_lock);

	if (found) {
		atomic_dec(&cbq->cb->refcnt);
		cn_queue_free_callback(cbq);
		atomic_dec(&dev->refcnt);
	}
}

struct cn_queue_dev *cn_queue_alloc_dev(char *name, struct sock *nls)
{
	struct cn_queue_dev *dev;

	dev = kmalloc(sizeof(*dev), GFP_KERNEL);
	if (!dev) {
		printk(KERN_ERR "%s: Failed to allocte new struct cn_queue_dev.\n",
		       name);
		return NULL;
	}

	memset(dev, 0, sizeof(*dev));

	snprintf(dev->name, sizeof(dev->name), "%s", name);

	atomic_set(&dev->refcnt, 0);
	INIT_LIST_HEAD(&dev->queue_list);
	spin_lock_init(&dev->queue_lock);

	dev->nls = nls;
	dev->netlink_groups = 0;

	dev->cn_queue = create_workqueue(dev->name);
	if (!dev->cn_queue) {
		printk(KERN_ERR "Failed to create %s queue.\n", dev->name);
		kfree(dev);
		return NULL;
	}

	return dev;
}

void cn_queue_free_dev(struct cn_queue_dev *dev)
{
	struct cn_callback_entry *cbq, *n;

	flush_workqueue(dev->cn_queue);
	destroy_workqueue(dev->cn_queue);

	spin_lock(&dev->queue_lock);
	list_for_each_entry_safe(cbq, n, &dev->queue_list, callback_entry) {
		list_del(&cbq->callback_entry);
		atomic_dec(&cbq->cb->refcnt);
	}
	spin_unlock(&dev->queue_lock);

	while (atomic_read(&dev->refcnt)) {
		printk(KERN_INFO "Waiting %s to became free: refcnt=%d.\n",
		       dev->name, atomic_read(&dev->refcnt));
		set_current_state(TASK_INTERRUPTIBLE);
		schedule_timeout(HZ);

		if (current->flags & PF_FREEZE)
			refrigerator(PF_FREEZE);

		if (signal_pending(current))
			flush_signals(current);
	}

	memset(dev, 0, sizeof(*dev));
	kfree(dev);
	dev = NULL;
}

EXPORT_SYMBOL(cn_queue_add_callback);
EXPORT_SYMBOL(cn_queue_del_callback);
EXPORT_SYMBOL(cn_queue_alloc_dev);
EXPORT_SYMBOL(cn_queue_free_dev);

[-- Attachment #1.3: cn_queue.h --]
[-- Type: text/x-chdr, Size: 2144 bytes --]

/*
 * 	cn_queue.h
 * 
 * 2004 Copyright (c) Evgeniy Polyakov <johnpol@2ka.mipt.ru>
 * 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#ifndef __CN_QUEUE_H
#define __CN_QUEUE_H

#include <asm/types.h>

struct cb_id
{
	__u32			idx;
	__u32			val;
};

#ifdef __KERNEL__

#include <asm/atomic.h>

#include <linux/list.h>
#include <linux/workqueue.h>

#define CN_CBQ_NAMELEN		32

struct cn_queue_dev
{
	atomic_t		refcnt;
	unsigned char		name[CN_CBQ_NAMELEN];

	struct workqueue_struct	*cn_queue;
	
	struct list_head 	queue_list;
	spinlock_t 		queue_lock;

	int			netlink_groups;
	struct sock		*nls;
};

struct cn_callback
{
	unsigned char		name[CN_CBQ_NAMELEN];
	
	struct cb_id		id;
	void			(* callback)(void *);
	void			*priv;
	
	atomic_t		refcnt;
};

struct cn_callback_entry
{
	struct list_head	callback_entry;
	struct cn_callback	*cb;
	struct work_struct	work;
	struct cn_queue_dev	*pdev;
	
	void			(* destruct_data)(void *);
	void			*ddata;

	int			seq, group;
	struct sock		*nls;
};

int cn_queue_add_callback(struct cn_queue_dev *dev, struct cn_callback *cb);
void cn_queue_del_callback(struct cn_queue_dev *dev, struct cn_callback *cb);

struct cn_queue_dev *cn_queue_alloc_dev(char *name, struct sock *);
void cn_queue_free_dev(struct cn_queue_dev *dev);

int cn_cb_equal(struct cb_id *, struct cb_id *);

#endif /* __KERNEL__ */
#endif /* __CN_QUEUE_H */

[-- Attachment #1.4: cn_test.c --]
[-- Type: text/x-csrc, Size: 1744 bytes --]

/*
 * 	cn_test.c
 * 
 * 2004 Copyright (c) Evgeniy Polyakov <johnpol@2ka.mipt.ru>
 * 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/moduleparam.h>

#include "connector.h"

static struct cb_id cn_test_id = { 0x123, 0x456 };
static char cn_test_name[] = "cn_test";

void cn_test_callback(void *data)
{
	struct cn_msg *msg = (struct cn_msg *)data;

	printk("%s: idx=%x, val=%x, len=%d.\n",
	       __func__, msg->id.idx, msg->id.val, msg->len);
}

static int cn_test_init(void)
{
	int err;

	err = cn_add_callback(&cn_test_id, cn_test_name, cn_test_callback);
	if (err)
		return err;
	cn_test_id.val++;
	err = cn_add_callback(&cn_test_id, cn_test_name, cn_test_callback);
	if (err) {
		cn_del_callback(&cn_test_id);
		return err;
	}

	return 0;
}

static void cn_test_fini(void)
{
	cn_del_callback(&cn_test_id);
	cn_test_id.val--;
	cn_del_callback(&cn_test_id);
}

module_init(cn_test_init);
module_exit(cn_test_fini);

[-- Attachment #1.5: connector.c --]
[-- Type: text/x-csrc, Size: 7632 bytes --]

/*
 * 	connector.c
 * 
 * 2004 Copyright (c) Evgeniy Polyakov <johnpol@2ka.mipt.ru>
 * 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/list.h>
#include <linux/skbuff.h>
#include <linux/netlink.h>
#include <linux/moduleparam.h>

#include <net/sock.h>

#include "../connector/connector.h"
#include "../connector/cn_queue.h"

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Evgeniy Polyakov <johnpol@2ka.mipt.ru>");
MODULE_DESCRIPTION("Generic userspace <-> kernelspace connector.");

static int unit = NETLINK_NFLOG;
module_param(unit, int, 0);

struct cn_dev cdev;

/*
 * msg->seq and msg->ack are used to determine message genealogy.
 * When someone sends message it puts there locally unique sequence 
 * and random acknowledge numbers.
 * Sequence number may be copied into nlmsghdr->nlmsg_seq too.
 *
 * Sequence number is incremented with each message to be sent.
 *
 * If we expect reply to our message, 
 * then sequence number in received message MUST be the same as in original message,
 * and acknowledge number MUST be the same + 1.
 *
 * If we receive message and it's sequence number is not equal to one we are expecting, 
 * then it is new message.
 * If we receive message and it's sequence number is the same as one we are expecting,
 * but it's acknowledge is not equal acknowledge number in original message + 1,
 * then it is new message.
 *
 */
void cn_netlink_send(struct cn_msg *msg)
{
	struct cn_callback_entry *n, *__cbq;
	unsigned int size;
	struct sk_buff *skb;
	struct nlmsghdr *nlh;
	struct cn_msg *data;
	struct cn_dev *dev = &cdev;
	u32 groups = 0;
	int found = 0;

	spin_lock(&dev->cbdev->queue_lock);
	list_for_each_entry_safe(__cbq, n, &dev->cbdev->queue_list, callback_entry) {
		if (cn_cb_equal(&__cbq->cb->id, &msg->id)) {
			found = 1;
			groups = __cbq->group;
		}
	}
	spin_unlock(&dev->cbdev->queue_lock);

	if (!found) {
		printk(KERN_ERR "Failed to find multicast netlink group for callback[0x%x.0x%x]. seq=%u\n",
		       msg->id.idx, msg->id.val, msg->seq);
		return;
	}

	size = NLMSG_SPACE(sizeof(*msg) + msg->len);

	skb = alloc_skb(size, GFP_ATOMIC);
	if (!skb) {
		printk(KERN_ERR "Failed to allocate new skb with size=%u.\n", size);
		return;
	}

	nlh = NLMSG_PUT(skb, 0, msg->seq, NLMSG_DONE, size - sizeof(*nlh));

	data = (struct cn_msg *)NLMSG_DATA(nlh);

	memcpy(data, msg, sizeof(*data) + msg->len);
#if 0
	printk("%s: len=%u, seq=%u, ack=%u, group=%u.\n",
	       __func__, msg->len, msg->seq, msg->ack, groups);
#endif
	NETLINK_CB(skb).dst_groups = groups;
	netlink_broadcast(dev->nls, skb, 0, groups, GFP_ATOMIC);

	return;

      nlmsg_failure:
	printk(KERN_ERR "Failed to send %u.%u\n", msg->seq, msg->ack);
	kfree_skb(skb);
	return;
}

static int cn_call_callback(struct cn_msg *msg, void (*destruct_data) (void *), void *data)
{
	struct cn_callback_entry *n, *__cbq;
	struct cn_dev *dev = &cdev;
	int found = 0;

	spin_lock(&dev->cbdev->queue_lock);
	list_for_each_entry_safe(__cbq, n, &dev->cbdev->queue_list, callback_entry) {
		if (cn_cb_equal(&__cbq->cb->id, &msg->id)) {
			__cbq->cb->priv = msg;

			__cbq->ddata = data;
			__cbq->destruct_data = destruct_data;

			queue_work(dev->cbdev->cn_queue, &__cbq->work);
			found = 1;
			break;
		}
	}
	spin_unlock(&dev->cbdev->queue_lock);

	return found;
}

static int __cn_rx_skb(struct sk_buff *skb, struct nlmsghdr *nlh)
{
	u32 pid, uid, seq, group;
	struct cn_msg *msg;

	pid = NETLINK_CREDS(skb)->pid;
	uid = NETLINK_CREDS(skb)->uid;
	seq = nlh->nlmsg_seq;
	group = NETLINK_CB((skb)).groups;
	msg = (struct cn_msg *)NLMSG_DATA(nlh);
#if 0
	printk(KERN_INFO "pid=%u, uid=%u, seq=%u, group=%u.\n",
	       pid, uid, seq, group);
#endif
	return cn_call_callback(msg, (void (*)(void *))kfree_skb, skb);
}

static void cn_rx_skb(struct sk_buff *__skb)
{
	struct nlmsghdr *nlh;
	u32 len;
	int err;
	struct sk_buff *skb;

	skb = skb_get(__skb);
	if (!skb) {
		printk(KERN_ERR "Failed to reference an skb.\n");
		return;
	}
#if 0
	printk(KERN_INFO
	       "skb: len=%u, data_len=%u, truesize=%u, proto=%u, cloned=%d, shared=%d.\n",
	       skb->len, skb->data_len, skb->truesize, skb->protocol,
	       skb_cloned(skb), skb_shared(skb));
#endif
	while (skb->len >= NLMSG_SPACE(0)) {
		nlh = (struct nlmsghdr *)skb->data;
		if (nlh->nlmsg_len < sizeof(struct cn_msg) ||
		    skb->len < nlh->nlmsg_len ||
		    nlh->nlmsg_len > CONNECTOR_MAX_MSG_SIZE) {
			printk(KERN_INFO "nlmsg_len=%u, sizeof(*nlh)=%u\n",
			       nlh->nlmsg_len, sizeof(*nlh));
			break;
		}

		len = NLMSG_ALIGN(nlh->nlmsg_len);
		if (len > skb->len)
			len = skb->len;

		err = __cn_rx_skb(skb, nlh);
		if (err) {
			if (err < 0)
				netlink_ack(skb, nlh, -err);
			kfree_skb(skb);
			break;
		} else {
			if (nlh->nlmsg_flags & NLM_F_ACK)
				netlink_ack(skb, nlh, 0);
			kfree_skb(skb);
			break;
		}
		skb_pull(skb, len);
	}
}

static void cn_input(struct sock *sk, int len)
{
	struct sk_buff *skb;

	while ((skb = skb_dequeue(&sk->sk_receive_queue)) != NULL)
		cn_rx_skb(skb);
}

int cn_add_callback(struct cb_id *id, char *name, void (*callback) (void *))
{
	int err;
	struct cn_dev *dev = &cdev;
	struct cn_callback *cb;

	cb = kmalloc(sizeof(*cb), GFP_KERNEL);
	if (!cb) {
		printk(KERN_INFO "%s: Failed to allocate new struct cn_callback.\n",
		       dev->cbdev->name);
		return -ENOMEM;
	}

	memset(cb, 0, sizeof(*cb));

	snprintf(cb->name, sizeof(cb->name), "%s", name);

	memcpy(&cb->id, id, sizeof(cb->id));
	cb->callback = callback;

	atomic_set(&cb->refcnt, 0);

	err = cn_queue_add_callback(dev->cbdev, cb);
	if (err) {
		kfree(cb);
		return err;
	}

	return 0;
}

void cn_del_callback(struct cb_id *id)
{
	struct cn_dev *dev = &cdev;
	struct cn_callback_entry *n, *__cbq;

	list_for_each_entry_safe(__cbq, n, &dev->cbdev->queue_list, callback_entry) {
		if (cn_cb_equal(&__cbq->cb->id, id)) {
			cn_queue_del_callback(dev->cbdev, __cbq->cb);
			break;
		}
	}
}

static int cn_init(void)
{
	struct cn_dev *dev = &cdev;

	dev->input = cn_input;

	dev->nls = netlink_kernel_create(unit, dev->input);
	if (!dev->nls) {
		printk(KERN_ERR "Failed to create new netlink socket(%u).\n",
		       unit);
		return -EIO;
	}

	dev->cbdev = cn_queue_alloc_dev("cqueue", dev->nls);
	if (!dev->cbdev) {
		if (dev->nls->sk_socket)
			sock_release(dev->nls->sk_socket);
		return -EINVAL;
	}

	return 0;
}

static void cn_fini(void)
{
	struct cn_dev *dev = &cdev;

	cn_queue_free_dev(dev->cbdev);
	if (dev->nls->sk_socket)
		sock_release(dev->nls->sk_socket);
}

module_init(cn_init);
module_exit(cn_fini);

EXPORT_SYMBOL(cn_add_callback);
EXPORT_SYMBOL(cn_del_callback);
EXPORT_SYMBOL(cn_netlink_send);

[-- Attachment #1.6: connector.h --]
[-- Type: text/x-chdr, Size: 1523 bytes --]

/*
 * 	connector.h
 * 
 * 2004 Copyright (c) Evgeniy Polyakov <johnpol@2ka.mipt.ru>
 * 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#ifndef __CONNECTOR_H
#define __CONNECTOR_H

#include "../connector/cn_queue.h"

#define CONNECTOR_MAX_MSG_SIZE 	1024

struct cn_msg
{
	struct cb_id 		id;

	__u32			seq;
	__u32			ack;

	__u32			len;		/* Length of the following data */
	__u8			data[0];
};

#ifdef __KERNEL__

#include <net/sock.h>

struct cn_dev
{
	u32			seq, groups;
	struct sock 		*nls;
	void 			(*input)(struct sock *sk, int len);

	struct cn_queue_dev	*cbdev;
};

int cn_add_callback(struct cb_id *, char *, void (* callback)(void *));
void cn_del_callback(struct cb_id *);
void cn_netlink_send(struct cn_msg *);

#endif /* __KERNEL__ */
#endif /* __CONNECTOR_H */

[-- Attachment #1.7: Makefile --]
[-- Type: text/x-makefile, Size: 364 bytes --]

obj-m		:= cn.o cn_test.o
cn-objs		:= cn_queue.o connector.o

#KDIR	:= /lib/modules/$(shell uname -r)/build
KDIR	:= /usr/local/src/linux-2.6/linux-2.6.6
PWD	:= $(shell pwd)

default:
	$(MAKE) -C $(KDIR) SUBDIRS=$(PWD) modules

clean:
	rm -f *.o *.ko *.mod.* .*.cmd *~
	rm -rf .tmp_versions

ucon:	ucon.c connector.h
	gcc -W -Wall -O9 ucon.c -o ucon

[-- Attachment #1.8: ucon.c --]
[-- Type: text/x-csrc, Size: 4532 bytes --]

/*
 * 	ucon.c
 *
 * Copyright (c) 2004 Evgeniy Polyakov <johnpol@2ka.mipt.ru>
 * 
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <asm/types.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/poll.h>

#include <linux/netlink.h>
#include <linux/rtnetlink.h>

#include <arpa/inet.h>

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <time.h>

#include "connector.h"
#include "../soekris/sc_conn.h"

static int need_exit;
__u32 seq;

static int netlink_send(int s, char *data)
{
	struct nlmsghdr *nlh;
	unsigned int size;
	int err;
	char buf[128];
	struct cn_msg *m, *msg;
	struct sc_conn_data *cmd;

	size = NLMSG_SPACE(sizeof(struct cn_msg) + sizeof(struct sc_conn_data));

	nlh = (struct nlmsghdr *)buf;
	nlh->nlmsg_seq = seq++;
	nlh->nlmsg_pid = getpid();
	nlh->nlmsg_type = NLMSG_DONE;
	nlh->nlmsg_len = NLMSG_LENGTH(size - sizeof(*nlh));
	nlh->nlmsg_flags = 0;

	m = NLMSG_DATA(nlh);
	msg = (struct cn_msg *)data;
	cmd = (struct sc_conn_data *)(msg + 1);

	printf("%s: len=%u, seq=%u, ack=%u, "
	       "sname=%s, lname=%s, idx=0x%x, cmd=%02x [%02x.%02x.%02x].\n",
	       __func__,
	       msg->len, msg->seq, msg->ack,
	       cmd->sname,
	       cmd->lname, cmd->idx, cmd->cmd, cmd->p0, cmd->p1, cmd->p2);

	memcpy(m, data, sizeof(*m) + msg->len);

	err = send(s, nlh, size, 0);
	if (err == -1)
		fprintf(stderr, "Failed to send: %s [%d].\n",
			strerror(errno), errno);

	return err;
}

int main(int argc, char *argv[])
{
	int s;
	char buf[1024];
	int len;
	struct nlmsghdr *reply;
	struct sockaddr_nl l_local;
	struct cn_msg *data;
	struct sc_conn_data *m;
	FILE *out;
	time_t tm;
	struct pollfd pfd;

	if (argc < 2)
		out = stdout;
	else {
		out = fopen(argv[1], "a+");
		if (!out) {
			fprintf(stderr, "Unable to open %s for writing: %s\n",
				argv[1], strerror(errno));
			out = stdout;
		}
	}

	memset(buf, 0, sizeof(buf));

	s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_NFLOG);
	if (s == -1) {
		perror("socket");
		return -1;
	}

	l_local.nl_family = AF_NETLINK;
	l_local.nl_groups = 1;
	l_local.nl_pid = getpid();

	if (bind(s, (struct sockaddr *)&l_local, sizeof(struct sockaddr_nl)) ==
	    -1) {
		perror("bind");
		close(s);
		return -1;
	}

	pfd.fd = s;

	while (!need_exit) {
		pfd.events = POLLIN;
		pfd.revents = 0;
		/*switch (poll(&pfd, 1, -1)) 
		   {
		   case 0:
		   need_exit = 1;
		   break;
		   case -1:
		   if (errno != EINTR) 
		   {
		   need_exit = 1;
		   break;
		   }
		   continue;
		   } */
		if (need_exit)
			break;

		data = (struct cn_msg *)buf;

		data->id.idx = 0xaabb;
		data->id.val = 0xccdd;
		data->seq = seq++;
		data->ack = (seq ^ 0x1234);
		data->len = sizeof(*m);

		m = (struct sc_conn_data *)(data + 1);
		memset(m, 0, sizeof(*m));

		m->cmd = SC_CMD_LDEV_READ;
		m->idx = 0xff;
		sprintf(m->sname, "PC8736X");
		sprintf(m->lname, "GPIO");

		m->p0 = 21;

		len = netlink_send(s, buf);

		len = recv(s, buf, sizeof(buf), 0);
		if (len == -1) {
			perror("recv buf");
			close(s);
			return -1;
		}
		reply = (struct nlmsghdr *)buf;

		switch (reply->nlmsg_type) {
		case NLMSG_ERROR:
			fprintf(out, "Error message received.\n");
			fflush(out);
			break;
		case NLMSG_DONE:
			data = (struct cn_msg *)NLMSG_DATA(reply);
			m = (struct sc_conn_data *)(data + 1);

			time(&tm);
			fprintf(out,
				"%.24s : [%x.%x] [seq=%u, ack=%u], sname=%s, lname=%s, idx=%u, cmd=%#02x [%#02x.%#02x.%#02x]\n",
				ctime(&tm), data->id.idx, data->id.val,
				data->seq, data->ack, m->sname, m->lname,
				m->idx, m->cmd, m->p0, m->p1, m->p2);
			fflush(out);
			break;
		default:
			break;
		}

		sleep(1);
	}

	close(s);
	return 0;
}

[-- Attachment #2: This is a digitally signed message part --]
[-- Type: application/pgp-signature, Size: 189 bytes --]