sdk/netlink_u.c

// Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <linux/version.h>
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
#include <linux/types.h>
#endif
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include "netlink_u.h"

#if !defined(NLMSG_HDRLEN)
#define NLMSG_HDRLEN	 ((int) NLMSG_ALIGN(sizeof(struct nlmsghdr)))
#endif

#define ND_MAX_GROUP			30
#define ND_IFINDEX_LEN			sizeof(int)
#define ND_NLMSG_SPACE(len)		(NLMSG_SPACE(len) + ND_IFINDEX_LEN)
#define ND_NLMSG_DATA(nlh)		((void *)((char *)NLMSG_DATA(nlh) + ND_IFINDEX_LEN))
#define ND_NLMSG_S_LEN(len)		(len+ND_IFINDEX_LEN)
#define ND_NLMSG_R_LEN(nlh)		(nlh->nlmsg_len-ND_IFINDEX_LEN)
#define ND_NLMSG_IFIDX(nlh)		NLMSG_DATA(nlh)
#define ND_MAX_MSG_LEN			8096

int nl_open(hnetlink_t *hnl, int unit, int ifindex, unsigned int group)
{
	struct sockaddr_nl nladdr;
	int fd;

	if (group > ND_MAX_GROUP) {
		fprintf(stderr, "ERROR: Group %d is invalied.\n", group);
		fprintf(stderr, "ERROR: Valid group is 0 ~ %d.\n", ND_MAX_GROUP);
		return -1;
	}

	fd = socket(AF_NETLINK, SOCK_RAW, unit);
	if (fd < 0) {
		fprintf(stderr, "ERROR: Cannot open netlink socket(%d). %s(%d)\n",
			unit, strerror(errno), errno);
		return -1;
	}

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;
	nladdr.nl_pid = 0;   /* For Linux Kernel */
	nladdr.nl_groups = 1;	/*For Multicase*/
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11)
	nladdr.nl_groups = group+1;
#else
	nladdr.nl_groups = 1 << group;
#endif

	if (bind(fd, (struct sockaddr *)&nladdr, sizeof(nladdr)) < 0) {
		fprintf(stderr, "ERROR: Cannot bind netlink socket. %s(%d)\n", strerror(errno), errno);
		close(fd);
		return -1;
	}

	hnl->fd = fd;
	hnl->ifindex = ifindex;
	return 0;
}

int nl_close(hnetlink_t *hnl)
{
	if (!hnl) {
		errno = EINVAL;
		return -1;
	}

	close(hnl->fd);
	memset(hnl, 0, sizeof(hnetlink_t));
	return 0;
}

int nl_send(hnetlink_t *hnl, unsigned short type, void *buf, int len)
{
	struct nlmsghdr *nlh;
	struct sockaddr_nl nladdr;
	struct iovec iov;
	struct msghdr msg = {(void *)&nladdr, sizeof(nladdr), &iov, 1, NULL, 0, 0};
	int fd, ret = -1;

	if (!hnl || !buf || !len) {
		errno = EINVAL;
		return -1;
	}

	fd = hnl->fd;

	if (ND_NLMSG_SPACE(len) > ND_MAX_MSG_LEN) {
		errno = EINVAL;
		return -1;
	}

	nlh = (struct nlmsghdr *) malloc(ND_NLMSG_SPACE(len));
 	nlh->nlmsg_len = ND_NLMSG_S_LEN(len);
	nlh->nlmsg_flags = 0;
	nlh->nlmsg_type = type;
	nlh->nlmsg_pid = 0;
	memcpy(ND_NLMSG_IFIDX(nlh), &hnl->ifindex, ND_IFINDEX_LEN);
	memcpy(ND_NLMSG_DATA(nlh), buf, len);

 	iov.iov_base = (void *) nlh;
	iov.iov_len = ND_NLMSG_SPACE(len);

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;

	ret = sendmsg(fd, &msg, 0);

	free(nlh);

	if (ret <= 0)
		return -1;

	return len;
}

int nl_recv(hnetlink_t *hnl, char *buf, int len, int flags)
{
	struct nlmsghdr *nlh;
	struct sockaddr_nl nladdr;
	struct iovec iov;
	struct msghdr msg = {(void *)&nladdr, sizeof(nladdr), &iov, 1, NULL, 0, 0};
	int fd, ret;

	if (!hnl || !buf || !len) {
		errno = EINVAL;
		return -1;
	}

	fd = hnl->fd;

	nlh = (struct nlmsghdr *) malloc(NLMSG_SPACE(len));
	memset(nlh, 0, NLMSG_SPACE(len));

	iov.iov_base = (void *)nlh;
	iov.iov_len = NLMSG_SPACE(len);

	memset(&nladdr, 0, sizeof(nladdr));
	nladdr.nl_family = AF_NETLINK;

	ret = recvmsg(fd, &msg, flags);

	if (ret > 0) {
		ret = nlh->nlmsg_len - NLMSG_HDRLEN;
		memcpy(buf, NLMSG_DATA(nlh), ret);
	}

	free(nlh);
	return ret;
}