There you go:
-----------------------------------------------------------------------------------
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/list.h>
#include <linux/workqueue.h>
#include <rtnet_rtpc.h>
#include <up.h>
static rtdm_lock_t umsg_list_lock = RTDM_LOCK_UNLOCKED;
static rtdm_nrtsig_t up_nrt_signal;
LIST_HEAD(umsg_list);
static void up_work_queue_handler(struct work_struct *work);
DECLARE_WORK(wq, up_work_queue_handler);
u32 up_user_pid = 0;
struct up_msg_buf *up_alloc_msg_buf(int cmd, size_t psize, struct
genl_info *info,
up_msg_finalize fin)
{
struct up_msg_buf *ret;
ret = kmalloc(sizeof(struct up_msg_buf) + psize, GFP_KERNEL);
if(!ret)
return ret;
memset(ret, 0, sizeof(struct up_msg_buf));
/* Initialize some fields */
if(info) {
ret->pid = info->snd_pid;
ret->seq = info->snd_seq;
}
ret->cmd = cmd;
ret->finalize = fin;
return ret;
}
void up_queue_umsg(struct up_msg_buf *umsg, int op, int len, U8 upid)
{
rtdm_lockctx_t context;
umsg->hdr.opcode = op;
umsg->hdr.plen = len;
umsg->hdr.up_id = upid;
rtdm_lock_get_irqsave(&umsg_list_lock, context);
list_add_tail(&umsg->list_entry, &umsg_list);
rtdm_lock_put_irqrestore(&umsg_list_lock, context);
rtdm_nrtsig_pend(&up_nrt_signal);
}
static int up_handle_cmd_msg_rt(struct rt_proc_call *call)
{
struct up_cmd_param *params;
struct up_msg_buf *resp;
params = rtpc_get_priv(call, struct up_cmd_param);
resp = params->resp_buf;
if(resp)
resp->cmd = UP_NL_C_CMD;
switch(params->hdr.opcode) {
case UP_CMD_INIT:
break;
case UP_CMD_CREATE_REQ:
{
struct up_config *c = (struct up_config *)¶ms->msg.config;
int *res = (int *)resp->payload;
*res = up_create(params->hdr.up_id, c);
if(*res < 0)
rtdm_printk("Error creating UP\n");
up_queue_umsg(resp, UP_CMD_CREATE_RES, sizeof(int), 0);
break;
}
default:
rtdm_printk("Unknown cmd message: op=%d\n", params->hdr.opcode);
return -ENOTSUPP;
}
return 0;
}
static int up_handle_cmd_msg_nrt(struct sk_buff *skb, struct genl_info
*info)
{
int ret;
struct up_cmd_param params;
struct up_user_hdr *uhdr = (struct up_user_hdr *)info->userhdr;
//TODO: Allocate response buffer based on message type
switch(uhdr->opcode) {
case UP_CMD_INIT:
up_user_pid = info->snd_pid;
params.resp_buf = NULL;
break;
default:
params.resp_buf = up_alloc_msg_buf(UP_NL_C_CMD,
sizeof(cmdMsg_t),
info, NULL);
break;
};
memcpy(¶ms.hdr, info->userhdr, sizeof(struct up_user_hdr));
if(params.hdr.plen > sizeof(cmdMsg_t))
printk("up_handle_cmd_msg_nrt(): ERROR plen=%u >
sizeof(cmdMsg_t)=%u\n", params.hdr.plen, sizeof(cmdMsg_t));
if(params.hdr.plen)
nla_memcpy(¶ms.msg, info->attrs[UP_NL_A_MSG], params.hdr.plen);
ret = rtpc_dispatch_call(up_handle_cmd_msg_rt, 0, ¶ms,
sizeof(params),
NULL, NULL, NULL);
if(ret < 0)
kfree(params.resp_buf);
return ret;
}
/* netlink attribute policy */
static const struct nla_policy up_genl_policy[UP_NL_A_MAX + 1] = {
[UP_NL_A_MSG] = { .type = NLA_BINARY }
};
/* Generic netlink event operation definition */
static struct genl_ops up_genl_ops_cmd = {
.cmd = UP_NL_C_CMD,
.flags = 0,
.policy = up_genl_policy,
.doit = up_handle_cmd_msg_nrt,
.dumpit = NULL
};
/* Generic netlink family */
static struct genl_family up_genl_family = {
.id = GENL_ID_GENERATE,
.hdrsize = UP_GENL_HDRLEN,
.name = "up",
.version = UP_NL_VERSION,
.maxattr = UP_NL_A_MAX
};
static void up_work_queue_handler(struct work_struct *work)
{
struct up_msg_buf *msg;
struct sk_buff *skb;
struct up_user_hdr *uhdr;
rtdm_lockctx_t context;
int rc;
rtdm_lock_get_irqsave(&umsg_list_lock, context);
while(!list_empty(&umsg_list)) {
msg = (struct up_msg_buf *)umsg_list.next;
list_del(&msg->list_entry);
rtdm_lock_put_irqrestore(&umsg_list_lock, context);
/* construct netlink message and send */
skb = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if(!skb)
goto failure;
uhdr = genlmsg_put(skb, msg->pid, msg->seq, &up_genl_family,
0, msg->cmd);
if(!uhdr)
goto skb_failure;
memcpy(uhdr, &msg->hdr, sizeof(struct up_user_hdr));
rc = nla_put(skb, UP_NL_A_MSG, msg->hdr.plen, &msg->payload);
if(rc != 0)
goto skb_failure;
genlmsg_end(skb, uhdr);
rc = genlmsg_unicast(skb, msg->pid);
if(msg->finalize)
msg->finalize(msg);
else
kfree(msg);
if(rc < 0)
goto skb_failure;
rtdm_lock_get_irqsave(&umsg_list_lock, context);
}
rtdm_lock_put_irqrestore(&umsg_list_lock, context);
return;
skb_failure:
nlmsg_free(skb);
failure:
printk("Response failure\n");
}
static void up_signal_handler(rtdm_nrtsig_t nrt_sig, void *arg)
{
/* Schedule work to be done in process context */
schedule_work(&wq);
}
static int __init up_init(void)
{
int rc = 0;
printk("UP module loading\n");
rc = genl_register_family(&up_genl_family);
if(rc != 0)
return rc;
rc = genl_register_ops(&up_genl_family, &up_genl_ops_cmd);
if(rc != 0)
goto failure;
rc = rtdm_nrtsig_init(&up_nrt_signal, up_signal_handler, NULL);
if(rc < 0)
goto failure;
rc = up_init();
if(rc < 0)
goto failure;
return 0;
failure:
printk("UP module loading failed\n");
genl_unregister_family(&up_genl_family);
return rc;
}
static void __exit up_release(void)
{
printk("UP module unloading\n");
rtdm_nrtsig_destroy(&up_nrt_signal);
genl_unregister_family(&up_genl_family);
up_release();
}
module_init(up_init);
module_exit(up_release);
-----------------------------------------------------------------------------------
/Jesper
On 2011-04-12 16:14, Jan Kiszka wrote:
> On 2011-04-12 16:09, Jesper Christensen wrote:
>
>> Speaking of rtpc, could there be a race condition when using a
>> rtdm_lock_t to synchronize between a linux thread and a xenomai thread?
>>
> Without seeing at some code, I can't comment on this meaningfully.
>
> Jan
>
>
_______________________________________________
Xenomai-core mailing list
[email protected]
https://mail.gna.org/listinfo/xenomai-core
