static void ntl_receive(struct sock *sk, int len)
{
int err;
struct sk_buff *skb;
pid_t pid;
struct nlmsghdr *nl_header;
__u32 seq;
struct pimfor_hdr *header;
char *data;
int version;
int operation;
unsigned long oid;
int dev_id;
int flags;
unsigned long length;
struct net_device *dev;
#ifdef DRIVER_DEBUG
printk(KERN_INFO "ntl_receive: sock %p, len %d \n", sk, len);
#endif
do
{
if (rtnl_shlock_nowait())
return;
while ( (skb = skb_dequeue(&sk->receive_queue)) != NULL )
{
pid = NETLINK_CB(skb).pid;
nl_header = (struct nlmsghdr*) skb->data;
seq = nl_header->nlmsg_seq;
header = (struct pimfor_hdr*)(skb->data+(sizeof(struct nlmsghdr)));
data = PIMFOR_DATA(header);
if ( nl_header->nlmsg_type == NETLINK_TYPE_PIMFOR )
{
pimfor_decode_header(header, &version, &operation, &oid, &dev_id, &flags, &length);
if (version == PIMFOR_VERSION_1) {
err = mgt_request( DEV_NETWORK, dev_id, pid, seq, operation, oid, data, length );
if ( err < 0 ) {
printk(KERN_INFO "ntl_receive: mgt_request(%d, %d, %d, %x, %d, %x, %d) returned %d\n",
DEV_NETWORK, dev_id, pid, seq, operation, oid, length, err );
netlink_ack(skb, nl_header, -EOPNOTSUPP );
}
} else {
printk(KERN_ERR "ntl_receive: version (%d) != PIMFOR_VERSION_1\n", version );
netlink_ack(skb, nl_header, -EOPNOTSUPP );
}
} else {
printk(KERN_ERR "nl_header->nlmsg_type (%d) != NETLINK_TYPE_PIMFOR\n", nl_header->nlmsg_type );
netlink_ack(skb, nl_header, -EOPNOTSUPP );
}
kfree_skb(skb);
}
up(&rtnl_sem);
} while (nl_sock && nl_sock->receive_queue.qlen);
}
static int audit_receive_msg(struct sk_buff *skb, struct nlmsghdr *nlh)
{
u32 uid, pid, seq, sid;
void *data;
struct audit_status *status_get, status_set;
int err;
struct audit_buffer *ab;
u16 msg_type = nlh->nlmsg_type;
uid_t loginuid; /* loginuid of sender */
struct audit_sig_info *sig_data;
char *ctx;
u32 len;
err = audit_netlink_ok(skb, msg_type);
if (err)
return err;
/* As soon as there's any sign of userspace auditd,
* start kauditd to talk to it */
if (!kauditd_task)
kauditd_task = kthread_run(kauditd_thread, NULL, "kauditd");
if (IS_ERR(kauditd_task)) {
err = PTR_ERR(kauditd_task);
kauditd_task = NULL;
return err;
}
pid = NETLINK_CREDS(skb)->pid;
uid = NETLINK_CREDS(skb)->uid;
loginuid = NETLINK_CB(skb).loginuid;
sid = NETLINK_CB(skb).sid;
seq = nlh->nlmsg_seq;
data = NLMSG_DATA(nlh);
switch (msg_type) {
case AUDIT_GET:
status_set.enabled = audit_enabled;
status_set.failure = audit_failure;
status_set.pid = audit_pid;
status_set.rate_limit = audit_rate_limit;
status_set.backlog_limit = audit_backlog_limit;
status_set.lost = atomic_read(&audit_lost);
status_set.backlog = skb_queue_len(&audit_skb_queue);
audit_send_reply(NETLINK_CB(skb).pid, seq, AUDIT_GET, 0, 0,
&status_set, sizeof(status_set));
break;
case AUDIT_SET:
if (nlh->nlmsg_len < sizeof(struct audit_status))
return -EINVAL;
status_get = (struct audit_status *)data;
if (status_get->mask & AUDIT_STATUS_ENABLED) {
err = audit_set_enabled(status_get->enabled,
loginuid, sid);
if (err < 0) return err;
}
if (status_get->mask & AUDIT_STATUS_FAILURE) {
err = audit_set_failure(status_get->failure,
loginuid, sid);
if (err < 0) return err;
}
if (status_get->mask & AUDIT_STATUS_PID) {
int old = audit_pid;
if (sid) {
if ((err = selinux_ctxid_to_string(
sid, &ctx, &len)))
return err;
else
audit_log(NULL, GFP_KERNEL,
AUDIT_CONFIG_CHANGE,
"audit_pid=%d old=%d by auid=%u subj=%s",
status_get->pid, old,
loginuid, ctx);
kfree(ctx);
} else
audit_log(NULL, GFP_KERNEL, AUDIT_CONFIG_CHANGE,
"audit_pid=%d old=%d by auid=%u",
status_get->pid, old, loginuid);
audit_pid = status_get->pid;
}
if (status_get->mask & AUDIT_STATUS_RATE_LIMIT)
err = audit_set_rate_limit(status_get->rate_limit,
loginuid, sid);
if (status_get->mask & AUDIT_STATUS_BACKLOG_LIMIT)
err = audit_set_backlog_limit(status_get->backlog_limit,
loginuid, sid);
break;
case AUDIT_USER:
case AUDIT_FIRST_USER_MSG...AUDIT_LAST_USER_MSG:
case AUDIT_FIRST_USER_MSG2...AUDIT_LAST_USER_MSG2:
if (!audit_enabled && msg_type != AUDIT_USER_AVC)
return 0;
err = audit_filter_user(&NETLINK_CB(skb), msg_type);
if (err == 1) {
err = 0;
ab = audit_log_start(NULL, GFP_KERNEL, msg_type);
if (ab) {
audit_log_format(ab,
"user pid=%d uid=%u auid=%u",
pid, uid, loginuid);
if (sid) {
if (selinux_ctxid_to_string(
sid, &ctx, &len)) {
audit_log_format(ab,
" ssid=%u", sid);
/* Maybe call audit_panic? */
} else
audit_log_format(ab,
" subj=%s", ctx);
kfree(ctx);
}
audit_log_format(ab, " msg='%.1024s'",
(char *)data);
audit_set_pid(ab, pid);
audit_log_end(ab);
}
}
break;
case AUDIT_ADD:
case AUDIT_DEL:
if (nlmsg_len(nlh) < sizeof(struct audit_rule))
return -EINVAL;
/* fallthrough */
case AUDIT_LIST:
err = audit_receive_filter(nlh->nlmsg_type, NETLINK_CB(skb).pid,
uid, seq, data, nlmsg_len(nlh),
loginuid, sid);
break;
case AUDIT_ADD_RULE:
case AUDIT_DEL_RULE:
if (nlmsg_len(nlh) < sizeof(struct audit_rule_data))
return -EINVAL;
/* fallthrough */
case AUDIT_LIST_RULES:
err = audit_receive_filter(nlh->nlmsg_type, NETLINK_CB(skb).pid,
uid, seq, data, nlmsg_len(nlh),
loginuid, sid);
break;
case AUDIT_SIGNAL_INFO:
err = selinux_ctxid_to_string(audit_sig_sid, &ctx, &len);
if (err)
return err;
sig_data = kmalloc(sizeof(*sig_data) + len, GFP_KERNEL);
if (!sig_data) {
kfree(ctx);
return -ENOMEM;
}
sig_data->uid = audit_sig_uid;
sig_data->pid = audit_sig_pid;
memcpy(sig_data->ctx, ctx, len);
kfree(ctx);
audit_send_reply(NETLINK_CB(skb).pid, seq, AUDIT_SIGNAL_INFO,
0, 0, sig_data, sizeof(*sig_data) + len);
kfree(sig_data);
break;
default:
err = -EINVAL;
break;
}
return err < 0 ? err : 0;
}
static int ntl_transmit(pid_t pid, int seq, int operation, int oid, long *data, unsigned long data_len,
int dev_type, int dev_id )
{
unsigned char *old_tail;
struct sk_buff *skb;
struct nlmsghdr *nlh;
struct pimfor_hdr *pimfor_hdr;
size_t size;
int err = 0;
/* Trap group is mapped to dev_type. */
unsigned int trapgrp = dev_type;
#ifdef DRIVER_DEBUG
printk(KERN_INFO "ntl_transmit: pid %d, seq %d, oper %d, oid %d, data ptr %p, data_len %lu, dev_id %d \n",
pid, seq, operation, oid, data, data_len, dev_id);
#endif
if ( nl_sock == NULL ) {
printk(KERN_ERR "ntl_transmit: no nl_sock\n");
return -EPFNOSUPPORT;
}
size = NLMSG_LENGTH(data_len + sizeof(struct pimfor_hdr));
skb = alloc_skb(size, GFP_ATOMIC);
if ( skb ) {
old_tail = skb->tail;
nlh = NLMSG_PUT(skb, pid, seq, NETLINK_TYPE_PIMFOR, size - NLMSG_ALIGN(sizeof(struct nlmsghdr)));
pimfor_hdr = (struct pimfor_hdr *)NLMSG_DATA(nlh);
pimfor_encode_header(operation, oid, dev_id, 0 /*flags*/, data_len, pimfor_hdr);
/* Add data after the PIMFOR header */
memcpy(PIMFOR_DATA(pimfor_hdr), data, data_len);
nlh->nlmsg_len = skb->tail - old_tail;
NETLINK_CB(skb).dst_groups = 0;
} else {
printk(KERN_ERR "ntl_transmit: alloc_skb failed\n");
return -EPFNOSUPPORT;
}
if (pid != 0 ) {
err = netlink_unicast(nl_sock, skb, pid, MSG_DONTWAIT);
} else {
if (trapgrp) {
netlink_broadcast(nl_sock, skb, pid, trapgrp ,GFP_ATOMIC );
}
else {
printk(KERN_WARNING "No trap group defined, drop packet.\n");
kfree_skb(skb);
}
}
return err;
nlmsg_failure:
if ( skb )
kfree_skb(skb);
printk(KERN_ERR "ntl_transmit: NLMSG_PUT failed\n");
return -EPFNOSUPPORT;
}
static char* process_crypto_request(u8 opcode, char* send_msg, int send_msg_size, int* result_len, int* ret) {
struct sk_buff *skb_in = NULL;
struct sk_buff *skb_out = NULL;
struct nlmsghdr *nlh = NULL;
//char* rev_msg = NULL;
char* result = NULL;
int rc = 0;
int ready = 0;
skb_in = nlmsg_new(send_msg_size, 0);
if (!skb_in) {
printk("%s %s Failed to allocate new skb: \n", PUB_CRYPTO_DEBUG, __FUNCTION__);
return NULL;
}
nlh = nlmsg_put(skb_in, 0, 0, NLMSG_DONE, send_msg_size, 0);
NETLINK_CB(skb_in).dst_group = 0;
memcpy(nlmsg_data(nlh), send_msg, send_msg_size);
mutex_lock(&crypto_send_mutex);
rc = nlmsg_unicast(crypto_sock, skb_in, user_fipscryptod_pid);
mutex_unlock(&crypto_send_mutex);
if (rc < 0) {
printk(KERN_INFO "Error while sending bak to user, err id: %d\n", rc);
return NULL;
}
//*******Reset the memory to hide plaintext!!!!!!
memset(send_msg, 0, send_msg_size);
{
DECLARE_WAITQUEUE(wait, current);
set_current_state(TASK_INTERRUPTIBLE);
add_wait_queue(&event_wait_crypto_results, &wait);
while (!ready) {
schedule_timeout_interruptible(CRYPTO_MAX_TIMEOUT);
mutex_lock(&crypto_data_ready_mutex);
ready = g_data_ready;
g_data_ready = 0;
mutex_unlock(&crypto_data_ready_mutex);
}
set_current_state(TASK_RUNNING);
remove_wait_queue(&event_wait_crypto_results, &wait);
skb_out = skb_dequeue(&crypto_sock->sk_receive_queue);
if(g_result_status == PUB_CRYPTO_ERROR) {
printk("%s %s FIPS_CRYPTO_ERROR!!!\n", PUB_CRYPTO_DEBUG, __FUNCTION__);
if(skb_out) {
kfree_skb(skb_out);
}
result = NULL; goto out;
}
if(g_result.ret == -1) {
printk("%s %s failed to opcode(%d)!!!\n", PUB_CRYPTO_DEBUG, __FUNCTION__, opcode);
result = NULL; goto out;
}
if(opcode == OP_RSA_ENC || opcode == OP_RSA_DEC ||
opcode == OP_DH_DEC || opcode == OP_DH_ENC) {
*result_len = sizeof(dek_t);
result = kmalloc(*result_len, GFP_KERNEL);
memcpy(result, &(g_result.dek), *result_len);
}
*ret = (int)g_result.ret;
}
out:
mutex_unlock(&crypto_receive_mutex);
return result;
}
INT MV_CC_ICCNetWrite(pMV_CC_ICCNet_t pHandle, UCHAR *pDatagramBuf)
{
int res;
MV_OSAL_CPUID_U8_t DstCpu;
unsigned long flags;
/* Parameter Check */
if (//(pHandle == NULL) ||
//(pDatagramBuf == NULL) ||
((DstCpu = ICCP_DstCPU(pDatagramBuf)) >= MV_OSAL_CPU_NUM))
MV_OSAL_ErrorHandle("MV_CC_ICCNetWrite", E_INVALIDARG);
if (DstCpu != MV_OSAL_CPU_LOCAL)
{
res = MV_CC_StartStatus_Check();
if (res == MV_CC_STATUS_OK)
{
PVOID pPtr;
pPtr = MV_CC_HAL_GetFIFOArrayAddr(&(pHandle->m_pHW));
if (pPtr == NULL)
MV_OSAL_ErrorHandle("MV_CC_ICCNetWrite MV_CC_HAL_GetFIFOArrayAddr pPtr", E_POINTER);
/* ICC FIFO Array Initialize */
res = MV_CC_ICCFIFOArrayInit(&(pHandle->m_FIFOArray), pPtr, MV_OSAL_CPU_LOCAL);
if (res < 0)
MV_OSAL_ErrorHandle("MV_CC_ICCNetWrite MV_CC_ICCFIFOArrayInit", res);
printk(KERN_ALERT "CPU-%d Check CC StartStatus = INITED\n", MV_OSAL_CPU_LOCAL);
}
else
if (res != S_OK)
return E_NOTREADY;
// Close interrupt
local_irq_save(flags);
res = MV_CC_ICCFIFOArrayWrite(&(pHandle->m_FIFOArray),
DstCpu,
pDatagramBuf);
switch (res)
{
case S_OK:
pHandle->m_WrCount++;
/* interrupt to Dst CPU */
res = MV_CC_HAL_SemPost(&(pHandle->m_pHW), DstCpu);
if (res != S_OK)
{
pHandle->m_WrSemErrCount++;
}
break;
case E_FULL:
pHandle->m_WrFullCount++;
break;
default:
break;
}
// Restore interrupt
local_irq_restore(flags);
}
else
{
struct sk_buff * skb = NULL;
struct nlmsghdr * nlhdr = NULL;
int len = MV_CC_ICCFIFO_FRAME_SIZE;
unsigned int dst_pid;
skb = nlmsg_new(len, GFP_KERNEL); //Linux Kernel 2.6.20
if (skb == NULL)
{
LOGPRINTF((KERN_ALERT "[MV_ICCNet] Can't create a netlink message.\n"));
return E_OUTOFMEMORY;
}
nlhdr = nlmsg_put(skb, 0, 0, 0, len, 0);
memcpy(NLMSG_DATA(nlhdr), pDatagramBuf, len);
NETLINK_CB(skb).pid = 0;
NETLINK_CB(skb).dst_group = NETLINK_GALOIS_CC_GROUP;
dst_pid = uiPidTable_GetLinuxPID(ICCP_DstProcess(pDatagramBuf));
if (dst_pid == 0)
{
// set MSG_DONTWAIT for a Nonblocking io, avoid sleep in netlink_broadcast
// set GFP_ATOMIC for a Nonblocking io, avoid sleep in netlink_broadcast
res = netlink_broadcast(pMV_ICCNet_netlink_sock, skb, 0, NETLINK_GALOIS_CC_GROUP, GFP_KERNEL);
}
else
{
res = netlink_unicast(pMV_ICCNet_netlink_sock, skb, dst_pid, MSG_DONTWAIT);
}
res = S_OK;
}
return res;
}
static int do_getlink(struct sk_buff *in_skb, struct nlmsghdr* in_nlh, void *arg)
{
struct ifinfomsg *ifm = NLMSG_DATA(in_nlh);
struct rtattr **ida = arg;
struct net_device *dev;
struct ifinfomsg *r;
struct nlmsghdr *nlh;
int err = -ENOBUFS;
struct sk_buff *skb;
unsigned char *b;
char *iw_buf = NULL;
int iw_buf_len = 0;
if (ifm->ifi_index >= 0)
dev = dev_get_by_index(ifm->ifi_index);
else
return -EINVAL;
if (!dev)
return -ENODEV;
#ifdef CONFIG_NET_WIRELESS_RTNETLINK
if (ida[IFLA_WIRELESS - 1]) {
/* Call Wireless Extensions. We need to know the size before
* we can alloc. Various stuff checked in there... */
err = wireless_rtnetlink_get(dev, RTA_DATA(ida[IFLA_WIRELESS - 1]), ida[IFLA_WIRELESS - 1]->rta_len, &iw_buf, &iw_buf_len);
if (err)
goto out;
}
#endif /* CONFIG_NET_WIRELESS_RTNETLINK */
/* Create a skb big enough to include all the data.
* Some requests are way bigger than 4k... Jean II */
skb = alloc_skb((NLMSG_LENGTH(sizeof(*r))) + (RTA_SPACE(iw_buf_len)),
GFP_KERNEL);
if (!skb)
goto out;
b = skb->tail;
/* Put in the message the usual good stuff */
nlh = NLMSG_PUT(skb, NETLINK_CB(in_skb).pid, in_nlh->nlmsg_seq,
RTM_NEWLINK, sizeof(*r));
r = NLMSG_DATA(nlh);
r->ifi_family = AF_UNSPEC;
r->__ifi_pad = 0;
r->ifi_type = dev->type;
r->ifi_index = dev->ifindex;
r->ifi_flags = dev->flags;
r->ifi_change = 0;
/* Put the wireless payload if it exist */
if(iw_buf != NULL)
RTA_PUT(skb, IFLA_WIRELESS, iw_buf_len,
iw_buf + IW_EV_POINT_OFF);
nlh->nlmsg_len = skb->tail - b;
/* Needed ? */
NETLINK_CB(skb).dst_pid = NETLINK_CB(in_skb).pid;
err = netlink_unicast(rtnl, skb, NETLINK_CB(in_skb).pid, MSG_DONTWAIT);
if (err > 0)
err = 0;
out:
if(iw_buf != NULL)
kfree(iw_buf);
dev_put(dev);
return err;
rtattr_failure:
nlmsg_failure:
kfree_skb(skb);
goto out;
}
/**
* kobject_uevent_env - send an uevent with environmental data
*
* @action: action that is happening (usually KOBJ_MOVE)
* @kobj: struct kobject that the action is happening to
* @envp_ext: pointer to environmental data
*
* Returns 0 if kobject_uevent() is completed with success or the
* corresponding error when it fails.
*/
int kobject_uevent_env(struct kobject *kobj, enum kobject_action action,
char *envp_ext[])
{
char **envp;
char *buffer;
char *scratch;
const char *action_string;
const char *devpath = NULL;
const char *subsystem;
struct kobject *top_kobj;
struct kset *kset;
struct kset_uevent_ops *uevent_ops;
u64 seq;
char *seq_buff;
int i = 0;
int retval = 0;
int j;
pr_debug("%s\n", __FUNCTION__);
action_string = action_to_string(action);
if (!action_string) {
pr_debug("kobject attempted to send uevent without action_string!\n");
return -EINVAL;
}
/* search the kset we belong to */
top_kobj = kobj;
if (!top_kobj->kset && top_kobj->parent) {
do {
top_kobj = top_kobj->parent;
} while (!top_kobj->kset && top_kobj->parent);
}
if (!top_kobj->kset) {
pr_debug("kobject attempted to send uevent without kset!\n");
return -EINVAL;
}
kset = top_kobj->kset;
uevent_ops = kset->uevent_ops;
/* skip the event, if the filter returns zero. */
if (uevent_ops && uevent_ops->filter)
if (!uevent_ops->filter(kset, kobj)) {
pr_debug("kobject filter function caused the event to drop!\n");
return 0;
}
/* environment index */
envp = kzalloc(NUM_ENVP * sizeof (char *), GFP_KERNEL);
if (!envp)
return -ENOMEM;
/* environment values */
buffer = kmalloc(BUFFER_SIZE, GFP_KERNEL);
if (!buffer) {
retval = -ENOMEM;
goto exit;
}
/* complete object path */
devpath = kobject_get_path(kobj, GFP_KERNEL);
if (!devpath) {
retval = -ENOENT;
goto exit;
}
/* originating subsystem */
if (uevent_ops && uevent_ops->name)
subsystem = uevent_ops->name(kset, kobj);
else
subsystem = kobject_name(&kset->kobj);
/* event environemnt for helper process only */
envp[i++] = "HOME=/";
envp[i++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
/* default keys */
scratch = buffer;
envp [i++] = scratch;
scratch += sprintf(scratch, "ACTION=%s", action_string) + 1;
envp [i++] = scratch;
scratch += sprintf (scratch, "DEVPATH=%s", devpath) + 1;
envp [i++] = scratch;
scratch += sprintf(scratch, "SUBSYSTEM=%s", subsystem) + 1;
for (j = 0; envp_ext && envp_ext[j]; j++)
envp[i++] = envp_ext[j];
/* just reserve the space, overwrite it after kset call has returned */
envp[i++] = seq_buff = scratch;
scratch += strlen("SEQNUM=18446744073709551616") + 1;
/* let the kset specific function add its stuff */
if (uevent_ops && uevent_ops->uevent) {
retval = uevent_ops->uevent(kset, kobj,
&envp[i], NUM_ENVP - i, scratch,
BUFFER_SIZE - (scratch - buffer));
if (retval) {
pr_debug ("%s - uevent() returned %d\n",
__FUNCTION__, retval);
goto exit;
}
}
/* we will send an event, request a new sequence number */
spin_lock(&sequence_lock);
seq = ++uevent_seqnum;
spin_unlock(&sequence_lock);
sprintf(seq_buff, "SEQNUM=%llu", (unsigned long long)seq);
#if defined(CONFIG_NET)
/* send netlink message */
if (uevent_sock) {
struct sk_buff *skb;
size_t len;
/* allocate message with the maximum possible size */
len = strlen(action_string) + strlen(devpath) + 2;
skb = alloc_skb(len + BUFFER_SIZE, GFP_KERNEL);
if (skb) {
/* add header */
scratch = skb_put(skb, len);
sprintf(scratch, "%s@%s", action_string, devpath);
/* copy keys to our continuous event payload buffer */
for (i = 2; envp[i]; i++) {
len = strlen(envp[i]) + 1;
scratch = skb_put(skb, len);
strcpy(scratch, envp[i]);
}
NETLINK_CB(skb).dst_group = 1;
netlink_broadcast(uevent_sock, skb, 0, 1, GFP_KERNEL);
}
}
#endif
/* call uevent_helper, usually only enabled during early boot */
if (uevent_helper[0]) {
char *argv [3];
argv [0] = uevent_helper;
argv [1] = (char *)subsystem;
argv [2] = NULL;
call_usermodehelper (argv[0], argv, envp, 0);
}
exit:
kfree(devpath);
kfree(buffer);
kfree(envp);
return retval;
}
/* HSR_C_GET_NODE_STATUS lets userspace query the internal HSR node table
* about the status of a specific node in the network, defined by its MAC
* address.
*
* Input: hsr ifindex, node mac address
* Output: hsr ifindex, node mac address (copied from request),
* age of latest frame from node over slave 1, slave 2 [ms]
*/
static int hsr_get_node_status(struct sk_buff *skb_in, struct genl_info *info)
{
/* For receiving */
struct nlattr *na;
struct net_device *hsr_dev;
/* For sending */
struct sk_buff *skb_out;
void *msg_head;
struct hsr_priv *hsr;
struct hsr_port *port;
unsigned char hsr_node_addr_b[ETH_ALEN];
int hsr_node_if1_age;
u16 hsr_node_if1_seq;
int hsr_node_if2_age;
u16 hsr_node_if2_seq;
int addr_b_ifindex;
int res;
if (!info)
goto invalid;
na = info->attrs[HSR_A_IFINDEX];
if (!na)
goto invalid;
na = info->attrs[HSR_A_NODE_ADDR];
if (!na)
goto invalid;
hsr_dev = __dev_get_by_index(genl_info_net(info),
nla_get_u32(info->attrs[HSR_A_IFINDEX]));
if (!hsr_dev)
goto invalid;
if (!is_hsr_master(hsr_dev))
goto invalid;
/* Send reply */
skb_out = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!skb_out) {
res = -ENOMEM;
goto fail;
}
msg_head = genlmsg_put(skb_out, NETLINK_CB(skb_in).portid,
info->snd_seq, &hsr_genl_family, 0,
HSR_C_SET_NODE_STATUS);
if (!msg_head) {
res = -ENOMEM;
goto nla_put_failure;
}
res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
if (res < 0)
goto nla_put_failure;
hsr = netdev_priv(hsr_dev);
res = hsr_get_node_data(hsr,
(unsigned char *) nla_data(info->attrs[HSR_A_NODE_ADDR]),
hsr_node_addr_b,
&addr_b_ifindex,
&hsr_node_if1_age,
&hsr_node_if1_seq,
&hsr_node_if2_age,
&hsr_node_if2_seq);
if (res < 0)
goto nla_put_failure;
res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN,
nla_data(info->attrs[HSR_A_NODE_ADDR]));
if (res < 0)
goto nla_put_failure;
if (addr_b_ifindex > -1) {
res = nla_put(skb_out, HSR_A_NODE_ADDR_B, ETH_ALEN,
hsr_node_addr_b);
if (res < 0)
goto nla_put_failure;
res = nla_put_u32(skb_out, HSR_A_ADDR_B_IFINDEX, addr_b_ifindex);
if (res < 0)
goto nla_put_failure;
}
res = nla_put_u32(skb_out, HSR_A_IF1_AGE, hsr_node_if1_age);
if (res < 0)
goto nla_put_failure;
res = nla_put_u16(skb_out, HSR_A_IF1_SEQ, hsr_node_if1_seq);
if (res < 0)
goto nla_put_failure;
rcu_read_lock();
port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
if (port)
res = nla_put_u32(skb_out, HSR_A_IF1_IFINDEX,
port->dev->ifindex);
rcu_read_unlock();
if (res < 0)
goto nla_put_failure;
res = nla_put_u32(skb_out, HSR_A_IF2_AGE, hsr_node_if2_age);
if (res < 0)
goto nla_put_failure;
res = nla_put_u16(skb_out, HSR_A_IF2_SEQ, hsr_node_if2_seq);
if (res < 0)
goto nla_put_failure;
rcu_read_lock();
port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
if (port)
res = nla_put_u32(skb_out, HSR_A_IF2_IFINDEX,
port->dev->ifindex);
rcu_read_unlock();
if (res < 0)
goto nla_put_failure;
genlmsg_end(skb_out, msg_head);
genlmsg_unicast(genl_info_net(info), skb_out, info->snd_portid);
return 0;
invalid:
netlink_ack(skb_in, nlmsg_hdr(skb_in), -EINVAL);
return 0;
nla_put_failure:
kfree_skb(skb_out);
/* Fall through */
fail:
return res;
}
/* Get a list of MacAddressA of all nodes known to this node (including self).
*/
static int hsr_get_node_list(struct sk_buff *skb_in, struct genl_info *info)
{
/* For receiving */
struct nlattr *na;
struct net_device *hsr_dev;
/* For sending */
struct sk_buff *skb_out;
void *msg_head;
struct hsr_priv *hsr;
void *pos;
unsigned char addr[ETH_ALEN];
int res;
if (!info)
goto invalid;
na = info->attrs[HSR_A_IFINDEX];
if (!na)
goto invalid;
hsr_dev = __dev_get_by_index(genl_info_net(info),
nla_get_u32(info->attrs[HSR_A_IFINDEX]));
if (!hsr_dev)
goto invalid;
if (!is_hsr_master(hsr_dev))
goto invalid;
/* Send reply */
skb_out = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
if (!skb_out) {
res = -ENOMEM;
goto fail;
}
msg_head = genlmsg_put(skb_out, NETLINK_CB(skb_in).portid,
info->snd_seq, &hsr_genl_family, 0,
HSR_C_SET_NODE_LIST);
if (!msg_head) {
res = -ENOMEM;
goto nla_put_failure;
}
res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
if (res < 0)
goto nla_put_failure;
hsr = netdev_priv(hsr_dev);
rcu_read_lock();
pos = hsr_get_next_node(hsr, NULL, addr);
while (pos) {
res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN, addr);
if (res < 0) {
rcu_read_unlock();
goto nla_put_failure;
}
pos = hsr_get_next_node(hsr, pos, addr);
}
rcu_read_unlock();
genlmsg_end(skb_out, msg_head);
genlmsg_unicast(genl_info_net(info), skb_out, info->snd_portid);
return 0;
invalid:
netlink_ack(skb_in, nlmsg_hdr(skb_in), -EINVAL);
return 0;
nla_put_failure:
kfree_skb(skb_out);
/* Fall through */
fail:
return res;
}
int cap_netlink_send (struct sk_buff *skb)
{
NETLINK_CB (skb).eff_cap = current->cap_effective;
return 0;
}
static int
netlink_trans_request(struct sk_buff *in_skb, struct genl_info *info)
{
char *buf;
unsigned int len;
uint32_t multi_flag;
struct nlmsghdr *rep, *nlh = info->nlhdr;
struct genlmsghdr *genlh;
struct nlattr **aap = info->attrs;
struct nlattr *nla;
struct vr_message request, *response;
struct sk_buff *skb;
if (!aap || !(nla = aap[NL_ATTR_VR_MESSAGE_PROTOCOL]))
return -EINVAL;
request.vr_message_buf = nla_data(nla);
request.vr_message_len = nla_len(nla);
vr_message_request(&request);
multi_flag = 0;
while ((response = vr_message_dequeue_response())) {
if ((multi_flag == 0) && (!vr_response_queue_empty()))
multi_flag = NLM_F_MULTI;
buf = response->vr_message_buf;
skb = netlink_skb(buf);
if (!skb)
continue;
len = response->vr_message_len;
len += GENL_HDRLEN + NLA_HDRLEN;
len = NLMSG_ALIGN(len);
rep = __nlmsg_put(skb, NETLINK_CB(in_skb).pid, nlh->nlmsg_seq,
nlh->nlmsg_type, len, multi_flag);
genlh = nlmsg_data(rep);
memcpy(genlh, info->genlhdr, sizeof(*genlh));
nla = (struct nlattr *)((char *)genlh + GENL_HDRLEN);
nla->nla_len = response->vr_message_len;
nla->nla_type = NL_ATTR_VR_MESSAGE_PROTOCOL;
netlink_unicast(in_skb->sk, skb,
NETLINK_CB(in_skb).pid, MSG_DONTWAIT);
response->vr_message_buf = NULL;
vr_message_free(response);
}
if (multi_flag) {
skb = alloc_skb(NLMSG_HDRLEN, GFP_ATOMIC);
if (!skb)
return 0;
__nlmsg_put(skb, NETLINK_CB(in_skb).pid, nlh->nlmsg_seq,
NLMSG_DONE, 0, 0);
netlink_unicast(in_skb->sk, skb,
NETLINK_CB(in_skb).pid, MSG_DONTWAIT);
}
return 0;
}
static int dcb_doit(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
{
struct net *net = sock_net(skb->sk);
struct net_device *netdev;
struct dcbmsg *dcb = (struct dcbmsg *)NLMSG_DATA(nlh);
struct nlattr *tb[DCB_ATTR_MAX + 1];
u32 pid = skb ? NETLINK_CB(skb).pid : 0;
int ret = -EINVAL;
if (net != &init_net)
return -EINVAL;
ret = nlmsg_parse(nlh, sizeof(*dcb), tb, DCB_ATTR_MAX,
dcbnl_rtnl_policy);
if (ret < 0)
return ret;
if (!tb[DCB_ATTR_IFNAME])
return -EINVAL;
netdev = dev_get_by_name(&init_net, nla_data(tb[DCB_ATTR_IFNAME]));
if (!netdev)
return -EINVAL;
if (!netdev->dcbnl_ops)
goto errout;
switch (dcb->cmd) {
case DCB_CMD_GSTATE:
ret = dcbnl_getstate(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PFC_GCFG:
ret = dcbnl_getpfccfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_GPERM_HWADDR:
ret = dcbnl_getperm_hwaddr(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PGTX_GCFG:
ret = dcbnl_pgtx_getcfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PGRX_GCFG:
ret = dcbnl_pgrx_getcfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_BCN_GCFG:
ret = dcbnl_bcn_getcfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_SSTATE:
ret = dcbnl_setstate(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PFC_SCFG:
ret = dcbnl_setpfccfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_SET_ALL:
ret = dcbnl_setall(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PGTX_SCFG:
ret = dcbnl_pgtx_setcfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PGRX_SCFG:
ret = dcbnl_pgrx_setcfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_GCAP:
ret = dcbnl_getcap(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_GNUMTCS:
ret = dcbnl_getnumtcs(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_SNUMTCS:
ret = dcbnl_setnumtcs(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PFC_GSTATE:
ret = dcbnl_getpfcstate(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_PFC_SSTATE:
ret = dcbnl_setpfcstate(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_BCN_SCFG:
ret = dcbnl_bcn_setcfg(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_GAPP:
ret = dcbnl_getapp(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
case DCB_CMD_SAPP:
ret = dcbnl_setapp(netdev, tb, pid, nlh->nlmsg_seq,
nlh->nlmsg_flags);
goto out;
default:
goto errout;
}
errout:
ret = -EINVAL;
out:
dev_put(netdev);
return ret;
}
int cap_netlink_recv(struct sk_buff *skb, int cap)
{
if (!cap_raised(NETLINK_CB(skb).eff_cap, cap))
return -EPERM;
return 0;
}
int cap_netlink_send(struct sock *sk, struct sk_buff *skb)
{
NETLINK_CB(skb).eff_cap = current_cap();
return 0;
}
//---------------------------------------------------------------------------
int nasmt_netlink_send(unsigned char *data_buffer, unsigned int data_length, int destination)
{
//---------------------------------------------------------------------------
struct sk_buff *nl_skb;
struct nlmsghdr *nlh;
int status;
// Start debug information
#ifdef NETLINK_DEBUG
printk("nasmt_netlink_send - begin \n");
#endif
if (!data_buffer) {
printk("nasmt_netlink_send - ERROR - input parameter data is NULL \n");
return(0);
}
if (!nas_nl_sk || !nas_rrcnl_sk) {
printk("nasmt_netlink_send - ERROR - socket is NULL\n");
return(0);
}
// End debug information
nl_skb = alloc_skb(NLMSG_SPACE(data_length),GFP_ATOMIC);
if (!nl_skb) {
printk("nasmt_netlink_send - ERROR - could not allocate skbuffer\n");
return(0);
}
nlh = (struct nlmsghdr *)nl_skb->data;
// printk("nasmt_netlink_send Sending %d bytes (%d)\n",data_length,NLMSG_SPACE(data_length));
skb_put(nl_skb, NLMSG_SPACE(data_length));
memcpy(NLMSG_DATA(nlh), data_buffer, data_length);
nlh->nlmsg_len = NLMSG_SPACE(data_length);
nlh->nlmsg_pid = 0; /* from kernel */
NETLINK_CB(nl_skb).pid = 0;
// destination 0 = PDCP, 1 = RRC
if (!destination) {
#ifdef NETLINK_DEBUG
printk("nasmt_netlink_send - nl_skb %p, nl_sk %p, nlh %p, nlh->nlmsg_len %d\n", nl_skb, nas_nl_sk, nlh, nlh->nlmsg_len);
#endif //DEBUG_NETLINK
status = netlink_unicast(nas_nl_sk, nl_skb, NL_DEST_PID, MSG_DONTWAIT);
} else {
#ifdef NAS_DEBUG_RRCNL
printk("nasmt_rrcnl_send - nl_skb %p, nas_rrcnl_sk %p, nlh %p, nlh->nlmsg_len %d\n", nl_skb, nas_rrcnl_sk, nlh, nlh->nlmsg_len);
#endif //NAS_DEBUG_RRCNL
status = netlink_unicast(nas_rrcnl_sk, nl_skb, NL_DEST_RRC_PID, MSG_DONTWAIT);
}
if (status < 0) {
printk("nasmt_netlink_send - SEND status is %d\n",status);
return(0);
} else {
#ifdef NETLINK_DEBUG
printk("nasmt_netlink_send - SEND status is %d, data_length %d\n",status, data_length);
#endif
return data_length;
}
}
static struct sk_buff *netlink_build_message(ipq_queue_element_t *e, int *errp)
{
unsigned char *old_tail;
size_t size = 0;
size_t data_len = 0;
struct sk_buff *skb;
ipq_packet_msg_t *pm;
struct nlmsghdr *nlh;
switch (nlq->peer.copy_mode) {
size_t copy_range;
case IPQ_COPY_META:
size = NLMSG_SPACE(sizeof(*pm));
data_len = 0;
break;
case IPQ_COPY_PACKET:
copy_range = nlq->peer.copy_range;
if (copy_range == 0 || copy_range > e->skb->len)
data_len = e->skb->len;
else
data_len = copy_range;
size = NLMSG_SPACE(sizeof(*pm) + data_len);
break;
case IPQ_COPY_NONE:
default:
*errp = -EINVAL;
return NULL;
}
skb = alloc_skb(size, GFP_ATOMIC);
if (!skb)
goto nlmsg_failure;
old_tail = skb->tail;
nlh = NLMSG_PUT(skb, 0, 0, IPQM_PACKET, size - sizeof(*nlh));
pm = NLMSG_DATA(nlh);
memset(pm, 0, sizeof(*pm));
pm->packet_id = (unsigned long )e;
pm->data_len = data_len;
pm->timestamp_sec = e->skb->stamp.tv_sec;
pm->timestamp_usec = e->skb->stamp.tv_usec;
pm->mark = e->skb->nfmark;
pm->hook = e->info->hook;
if (e->info->indev) strcpy(pm->indev_name, e->info->indev->name);
else pm->indev_name[0] = '\0';
if (e->info->outdev) strcpy(pm->outdev_name, e->info->outdev->name);
else pm->outdev_name[0] = '\0';
pm->hw_protocol = e->skb->protocol;
if (e->info->indev && e->skb->dev) {
pm->hw_type = e->skb->dev->type;
if (e->skb->dev->hard_header_parse)
pm->hw_addrlen =
e->skb->dev->hard_header_parse(e->skb,
pm->hw_addr);
}
if (data_len)
memcpy(pm->payload, e->skb->data, data_len);
nlh->nlmsg_len = skb->tail - old_tail;
NETLINK_CB(skb).dst_groups = 0;
return skb;
nlmsg_failure:
if (skb)
kfree_skb(skb);
*errp = 0;
printk(KERN_ERR "ip_queue: error creating netlink message\n");
return NULL;
}
static int rtnetlink_fill_ifinfo(struct sk_buff *skb, struct net_device *dev,
int type, u32 pid, u32 seq, u32 change,
unsigned int flags)
{
struct ifinfomsg *r;
struct nlmsghdr *nlh;
unsigned char *b = skb->tail;
nlh = NLMSG_NEW(skb, pid, seq, type, sizeof(*r), flags);
r = NLMSG_DATA(nlh);
r->ifi_family = AF_UNSPEC;
r->__ifi_pad = 0;
r->ifi_type = dev->type;
r->ifi_index = dev->ifindex;
r->ifi_flags = dev_get_flags(dev);
r->ifi_change = change;
RTA_PUT(skb, IFLA_IFNAME, strlen(dev->name)+1, dev->name);
if (1) {
u32 txqlen = dev->tx_queue_len;
RTA_PUT(skb, IFLA_TXQLEN, sizeof(txqlen), &txqlen);
}
if (1) {
u32 weight = dev->weight;
RTA_PUT(skb, IFLA_WEIGHT, sizeof(weight), &weight);
}
if (1) {
u8 operstate = netif_running(dev)?dev->operstate:IF_OPER_DOWN;
u8 link_mode = dev->link_mode;
RTA_PUT(skb, IFLA_OPERSTATE, sizeof(operstate), &operstate);
RTA_PUT(skb, IFLA_LINKMODE, sizeof(link_mode), &link_mode);
}
if (1) {
struct rtnl_link_ifmap map = {
.mem_start = dev->mem_start,
.mem_end = dev->mem_end,
.base_addr = dev->base_addr,
.irq = dev->irq,
.dma = dev->dma,
.port = dev->if_port,
};
RTA_PUT(skb, IFLA_MAP, sizeof(map), &map);
}
if (dev->addr_len) {
RTA_PUT(skb, IFLA_ADDRESS, dev->addr_len, dev->dev_addr);
RTA_PUT(skb, IFLA_BROADCAST, dev->addr_len, dev->broadcast);
}
if (1) {
u32 mtu = dev->mtu;
RTA_PUT(skb, IFLA_MTU, sizeof(mtu), &mtu);
}
if (dev->ifindex != dev->iflink) {
u32 iflink = dev->iflink;
RTA_PUT(skb, IFLA_LINK, sizeof(iflink), &iflink);
}
if (dev->qdisc_sleeping)
RTA_PUT(skb, IFLA_QDISC,
strlen(dev->qdisc_sleeping->ops->id) + 1,
dev->qdisc_sleeping->ops->id);
if (dev->master) {
u32 master = dev->master->ifindex;
RTA_PUT(skb, IFLA_MASTER, sizeof(master), &master);
}
if (dev->get_stats) {
unsigned long *stats = (unsigned long*)dev->get_stats(dev);
if (stats) {
struct rtattr *a;
__u32 *s;
int i;
int n = sizeof(struct rtnl_link_stats)/4;
a = __RTA_PUT(skb, IFLA_STATS, n*4);
s = RTA_DATA(a);
for (i=0; i<n; i++)
s[i] = stats[i];
}
}
nlh->nlmsg_len = skb->tail - b;
return skb->len;
nlmsg_failure:
rtattr_failure:
skb_trim(skb, b - skb->data);
return -1;
}
static int rtnetlink_dump_ifinfo(struct sk_buff *skb, struct netlink_callback *cb)
{
int idx;
int s_idx = cb->args[0];
struct net_device *dev;
read_lock(&dev_base_lock);
for (dev=dev_base, idx=0; dev; dev = dev->next, idx++) {
if (idx < s_idx)
continue;
if (rtnetlink_fill_ifinfo(skb, dev, RTM_NEWLINK,
NETLINK_CB(cb->skb).pid,
cb->nlh->nlmsg_seq, 0,
NLM_F_MULTI) <= 0)
break;
}
read_unlock(&dev_base_lock);
cb->args[0] = idx;
return skb->len;
}
static int
itson_rcv_msg(struct sk_buff *skb_2, struct genl_info *info)
{
struct nlattr *na;
struct msgInfo minfo;
int rc = 0;
void *msg;
int index;
if (info == NULL) {
rc = -EINVAL;
goto rcv_msg_err;
}
index = info->genlhdr->cmd;
/* Ignore control command */
if (index < CTRL_CMD_MAX) {
INFO("Received control msg %d, skipping\n", index);
return 0;
}
// Ignore out-of-range commands (not ours, so print an error).
if (index < ITSON_CMD_0 || index > ITSON_CMD_MAX) {
ERR("Received out-of-range netlink message %d, skipping\n", index);
return 0;
}
//INFO("%s cmd %d", __func__, index);
na = info->attrs[index];
if (na == NULL) {
printk("no info->attrs %i\n", index);
rc = -EINVAL;
goto rcv_msg_err;
}
msg = nla_data(na);
if (msg == NULL) {
printk("error while receiving data\n");
rc = -EINVAL;
goto rcv_msg_err;
}
//INFO("cmd %d len %d\n", index, na->nla_len);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,6,0)
minfo.pid = info->snd_portid;
#else
minfo.pid = info->snd_pid;
#endif
minfo.uid = NETLINK_CB(skb_2).creds.uid;
minfo.gid = NETLINK_CB(skb_2).creds.gid;
if (msgHandlerList[index]) {
rc = msgHandlerList[index](msg, nla_len(na), &minfo);
} else {
ERR("No handler for found cmd %d", index);
rc = -EINVAL;
goto rcv_msg_err;
}
rcv_msg_err:
return rc;
}
VOS_INT DMS_NLK_Send(
DMS_NLK_PHY_BEAR_ENUM_UINT32 enPhyBear,
DMS_NLK_MSG_TYPE_ENUM_UINT32 enMsgType,
VOS_UINT8 *pucData,
VOS_UINT32 ulLength
)
{
struct sk_buff *pstSkb = VOS_NULL_PTR;
struct nlmsghdr *pstNlkHdr = VOS_NULL_PTR;
DMS_NLK_PAYLOAD_STRU *pstPlayload = VOS_NULL_PTR;
VOS_UINT ulMsgSize;
VOS_UINT ulPayloadSize;
VOS_INT lRet;
DMS_DBG_NLK_DL_TOTAL_MSG_NUM(1);
/* 检查 netlink socket */
if (VOS_NULL_PTR == DMS_GET_NLK_SOCK())
{
DMS_DBG_NLK_DL_ERR_SOCK_MSG_NUM(1);
return -EIO;
}
/* 检查物理承载状态 */
if (DMS_NLK_INVALID_PID == DMS_GET_NLK_PHY_PID(enPhyBear))
{
DMS_DBG_NLK_DL_ERR_PID_MSG_NUM(1);
return -EINVAL;
}
/* 申请 netlink 消息 */
ulPayloadSize = sizeof(DMS_NLK_PAYLOAD_STRU) + ulLength;
ulMsgSize = NLMSG_SPACE(ulPayloadSize);
pstSkb = nlmsg_new(ulPayloadSize, GFP_ATOMIC);
if (VOS_NULL_PTR == pstSkb)
{
DMS_DBG_NLK_DL_ALLOC_MSG_FAIL_NUM(1);
return -ENOBUFS;
}
/* 填充 netlink 消息头 */
/* Use "ulMsgSize - sizeof(*pstNlkHdr)" here (incluing align pads) */
pstNlkHdr = nlmsg_put(pstSkb, 0, 0, (VOS_INT)enMsgType,
(VOS_INT)(ulMsgSize - sizeof(struct nlmsghdr)), 0);
if (VOS_NULL_PTR == pstNlkHdr)
{
kfree_skb(pstSkb);
DMS_DBG_NLK_DL_PUT_MSG_FAIL_NUM(1);
return -EMSGSIZE;
}
/* 填充 netlink 消息接收PID */
/*lint -e545*/
#if ((LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0)) || (VOS_OS_VER == VOS_WIN32))
NETLINK_CB(pstSkb).portid = DMS_GET_NLK_PHY_PID(enPhyBear);
#else
NETLINK_CB(pstSkb).pid = DMS_GET_NLK_PHY_PID(enPhyBear);
#endif
NETLINK_CB(pstSkb).dst_group = 0;
/*lint +e545*/
/* 填充 netlink 消息内容 */
pstPlayload = nlmsg_data(pstNlkHdr);
pstPlayload->ulLength = ulLength;
memcpy(pstPlayload->aucData, pucData, ulLength);
/* 发送 netlink 消息 */
/*lint -e545*/
#if ((LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0)) || (VOS_OS_VER == VOS_WIN32))
lRet = netlink_unicast(DMS_GET_NLK_SOCK(), pstSkb, NETLINK_CB(pstSkb).portid, 0);
#else
lRet = netlink_unicast(DMS_GET_NLK_SOCK(), pstSkb, NETLINK_CB(pstSkb).pid, 0);
#endif
/*lint +e545*/
if (lRet < 0)
{
DMS_DBG_NLK_DL_UNICAST_MSG_FAIL_NUM(1);
return lRet;
}
DMS_DBG_NLK_DL_UNICAST_MSG_SUCC_NUM(1);
return 0;
}
static int dummy_netlink_recv (struct sk_buff *skb)
{
if (!cap_raised (NETLINK_CB (skb).eff_cap, CAP_NET_ADMIN))
return -EPERM;
return 0;
}
static int
tc_dump_action(struct sk_buff *skb, struct netlink_callback *cb)
{
struct net *net = sock_net(skb->sk);
struct nlmsghdr *nlh;
unsigned char *b = skb_tail_pointer(skb);
struct nlattr *nest;
struct tc_action_ops *a_o;
struct tc_action a;
int ret = 0;
struct tcamsg *t = (struct tcamsg *) NLMSG_DATA(cb->nlh);
struct nlattr *kind = find_dump_kind(cb->nlh);
if (net != &init_net)
return 0;
if (kind == NULL) {
printk("tc_dump_action: action bad kind\n");
return 0;
}
a_o = tc_lookup_action(kind);
if (a_o == NULL) {
return 0;
}
memset(&a, 0, sizeof(struct tc_action));
a.ops = a_o;
if (a_o->walk == NULL) {
printk("tc_dump_action: %s !capable of dumping table\n", a_o->kind);
goto nla_put_failure;
}
nlh = NLMSG_PUT(skb, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
cb->nlh->nlmsg_type, sizeof(*t));
t = NLMSG_DATA(nlh);
t->tca_family = AF_UNSPEC;
t->tca__pad1 = 0;
t->tca__pad2 = 0;
nest = nla_nest_start(skb, TCA_ACT_TAB);
if (nest == NULL)
goto nla_put_failure;
ret = a_o->walk(skb, cb, RTM_GETACTION, &a);
if (ret < 0)
goto nla_put_failure;
if (ret > 0) {
nla_nest_end(skb, nest);
ret = skb->len;
} else
nla_nest_cancel(skb, nest);
nlh->nlmsg_len = skb_tail_pointer(skb) - b;
if (NETLINK_CB(cb->skb).pid && ret)
nlh->nlmsg_flags |= NLM_F_MULTI;
module_put(a_o->owner);
return skb->len;
nla_put_failure:
nlmsg_failure:
module_put(a_o->owner);
nlmsg_trim(skb, b);
return skb->len;
}
static int
nfnl_compat_get(struct sock *nfnl, struct sk_buff *skb,
const struct nlmsghdr *nlh, const struct nlattr * const tb[])
{
int ret = 0, target;
struct nfgenmsg *nfmsg;
const char *fmt;
const char *name;
u32 rev;
struct sk_buff *skb2;
if (tb[NFTA_COMPAT_NAME] == NULL ||
tb[NFTA_COMPAT_REV] == NULL ||
tb[NFTA_COMPAT_TYPE] == NULL)
return -EINVAL;
name = nla_data(tb[NFTA_COMPAT_NAME]);
rev = ntohl(nla_get_be32(tb[NFTA_COMPAT_REV]));
target = ntohl(nla_get_be32(tb[NFTA_COMPAT_TYPE]));
nfmsg = nlmsg_data(nlh);
switch(nfmsg->nfgen_family) {
case AF_INET:
fmt = "ipt_%s";
break;
case AF_INET6:
fmt = "ip6t_%s";
break;
default:
pr_err("nft_compat: unsupported protocol %d\n",
nfmsg->nfgen_family);
return -EINVAL;
}
try_then_request_module(xt_find_revision(nfmsg->nfgen_family, name,
rev, target, &ret),
fmt, name);
if (ret < 0)
return ret;
skb2 = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
if (skb2 == NULL)
return -ENOMEM;
/* include the best revision for this extension in the message */
if (nfnl_compat_fill_info(skb2, NETLINK_CB(skb).portid,
nlh->nlmsg_seq,
NFNL_MSG_TYPE(nlh->nlmsg_type),
NFNL_MSG_COMPAT_GET,
nfmsg->nfgen_family,
name, ret, target) <= 0) {
kfree_skb(skb2);
return -ENOSPC;
}
ret = netlink_unicast(nfnl, skb2, NETLINK_CB(skb).portid,
MSG_DONTWAIT);
if (ret > 0)
ret = 0;
return ret == -EAGAIN ? -ENOBUFS : ret;
}
