C++ NLA_ALIGN函数代码示例

static struct nlattr *
nla_next(struct nlattr *nla, size_t *rem)
{

	*rem -= (size_t)NLA_ALIGN(nla->nla_len);
	return (void *)((char *)nla + NLA_ALIGN(nla->nla_len));
}

int set_hw_app0(char *ifname, appgroup_attribs *app_data)
{
	struct nlmsghdr *nlh;
	struct rtattr *rta_parent, *rta_child;

	printf("set_hw_app0: %s\n", ifname);

	nlh = start_msg(RTM_SETDCB, DCB_CMD_SAPP);
	if (NULL == nlh)
		return -EIO;

	add_rta(nlh, DCB_ATTR_IFNAME, (void *)ifname, strlen(ifname) + 1);
	rta_parent = add_rta(nlh, DCB_ATTR_APP, NULL, 0);

	rta_child = add_rta(nlh, DCB_APP_ATTR_IDTYPE,
		(void *)&app_data->dcb_app_idtype, sizeof(__u8));
	rta_parent->rta_len += NLA_ALIGN(rta_child->rta_len);

	rta_child = add_rta(nlh, DCB_APP_ATTR_ID,
		(void *)&app_data->dcb_app_id, sizeof(__u16));
	rta_parent->rta_len += NLA_ALIGN(rta_child->rta_len);

	rta_child = add_rta(nlh, DCB_APP_ATTR_PRIORITY,
		(void *)&app_data->dcb_app_priority, sizeof(__u8));
	rta_parent->rta_len += NLA_ALIGN(rta_child->rta_len);

	if (send_msg(nlh))
		return -EIO;

	return(recv_msg(DCB_CMD_SAPP, DCB_ATTR_APP));
}

void cputimer_get(int tid, long long int *times, xbt_cpu_timer_t timer)
{
	struct msgtemplate msg;
	int rep_len;
	struct nlattr *na;
	struct taskstats *stats;

	netlink_sock_send(timer->_id, TASKSTATS_CMD_GET, TASKSTATS_CMD_ATTR_PID, &tid, sizeof(__u32), timer);

	rep_len = recv(timer->_nl_sd, &msg, sizeof(msg), 0);
	xbt_assert(rep_len >= 0, "error while receiving the answer from netlink socket: %s", strerror(errno));

	xbt_assert((msg.n.nlmsg_type != NLMSG_ERROR && NLMSG_OK((&msg.n), rep_len)),
			"received a fatal error from the netlink socket: %s", strerror(errno));

	rep_len = GENLMSG_PAYLOAD(&msg.n);

	na = (struct nlattr *) GENLMSG_DATA(&msg);
	int len = 0;
	int aggr_len, len2;
	while (len < rep_len) {
		len += NLA_ALIGN(na->nla_len);
		switch (na->nla_type) {
		case TASKSTATS_TYPE_AGGR_PID:
			aggr_len = NLA_PAYLOAD(na->nla_len);
			len2 = 0;
			/* For nested attributes, na follows */
			na = (struct nlattr *) NLA_DATA(na);
			while (len2 < aggr_len) {
				switch (na->nla_type) {
				case TASKSTATS_TYPE_PID:
					break;
				case TASKSTATS_TYPE_STATS:
					/* here we collect info */
					stats = (struct taskstats *) NLA_DATA(na);
					//times[0] = (long long int)stats->ac_etime;
					times[1] = (long long int) stats->ac_utime;   /* User CPU time [usec] */
					times[2] = (long long int) stats->ac_stime;   /* SYstem CPU time [usec] */
					break;
				default:
					XBT_ERROR("Unknown nested" " nla_type %d\n", na->nla_type);
					break;
				}
				len2 += NLA_ALIGN(na->nla_len);
				na = (struct nlattr *) ((char *) na + len2);
			}
			break;
		default:
			XBT_ERROR("Unknown nla_type %d\n", na->nla_type);
			break;
		}
		na = (struct nlattr *) ((char *) GENLMSG_DATA(&msg) + len);
	}
}

	static void
parse_groups(struct nlattr *na, int tot_len)
{
	int	len;
	int	grp_len;
	int	aggr_len;
	struct nlattr *grp_na;
	int find = 0;
	char group_name[GENL_NAMSIZ] = {0};
	int group_id = -1;

	len = 0;
	while (len < tot_len) {
		len += NLA_ALIGN(na->nla_len);
		printf("grp #%02d\n", na->nla_type);
#if 0
		if (na->nla_type > 1) {
			/* only one group supported for now */
			//na = (struct nlattr *) ((char *) na + len);
			//continue;
			printf("!!multi group, loop to find!!\n");
		}
#endif
		aggr_len = NLA_PAYLOAD_LEN(na->nla_len);
		grp_na = (struct nlattr *) NLA_DATA(na);
		grp_len = 0;
		while (grp_len < aggr_len) {
			grp_len += NLA_ALIGN(grp_na->nla_len);
			switch (grp_na->nla_type) {
				case CTRL_ATTR_MCAST_GRP_ID:
					group_id = *(uint32_t *) NLA_DATA(grp_na);
					printf("grp id = %d\n", nl_grp_id);
					break;
				case CTRL_ATTR_MCAST_GRP_NAME:
					strcpy(group_name, (char *)NLA_DATA(grp_na));
					printf("grp name %s\n", group_name);
					break;
				default:
					printf("Unknown grp nested attr %d\n", 	grp_na->nla_type);
					break;
			}
			grp_na = (struct nlattr *) ((char *) grp_na + grp_len);
		}
		if (-1 != group_id && strlen(group_name) > 0) {
			if (0 == strcmp(group_name, nl_grp_name)) {
				printf("find grp name:%s, id=%d\n", group_name, group_id);
				nl_grp_id = group_id;
				break;
			}
		}
		na = (struct nlattr *) ((char *) na + len);
	}
}

static int parse_attr(struct nlattr *tb[], int maxtype, struct nlattr *nla,
		      const struct nla_policy *policy, int len)
{
	int nested_len = nla_len(nla) - NLA_ALIGN(len);

	if (nested_len < 0)
		return -EINVAL;
	if (nested_len >= nla_attr_size(0))
		return nla_parse(tb, maxtype, nla_data(nla) + NLA_ALIGN(len),
				 nested_len, policy);
	memset(tb, 0, sizeof(struct nlattr *) * (maxtype + 1));
	return 0;
}

static struct taskstats *
wait_taskstats (void)
{
	static struct msgtemplate msg;
	int rep_len;

	for (;;) {
		while ((rep_len = recv (netlink_socket, &msg, sizeof(msg), 0)) < 0 && errno == EINTR);
  
		if (msg.n.nlmsg_type == NLMSG_ERROR ||
		    !NLMSG_OK((&msg.n), rep_len)) {
			/* process died before we got to it or somesuch */
			/* struct nlmsgerr *err = NLMSG_DATA(&msg);
			   fprintf (stderr, "fatal reply error,  errno %d\n", err->error); */
			return NULL;
		}
  
		int rep_len = GENLMSG_PAYLOAD(&msg.n);
		struct nlattr *na = (struct nlattr *) GENLMSG_DATA(&msg);
		int len = 0;
		
		while (len < rep_len) {
			len += NLA_ALIGN(na->nla_len);
			switch (na->nla_type) {
			case TASKSTATS_TYPE_AGGR_PID: {
				int aggr_len = NLA_PAYLOAD(na->nla_len);
				int len2 = 0;

				/* For nested attributes, na follows */
				na = (struct nlattr *) NLA_DATA(na);
				
				/* find the record we care about */
				while (na->nla_type != TASKSTATS_TYPE_STATS) {
					len2 += NLA_ALIGN(na->nla_len);

					if (len2 >= aggr_len)
						goto next_attr;
					na = (struct nlattr *) ((char *) na + len2);
				}
				return (struct taskstats *) NLA_DATA(na);
			}
			}
		next_attr:
			na = (struct nlattr *) (GENLMSG_DATA(&msg) + len);
		}
	}
	return NULL;
}

static int flashnode_fill_isid(struct flashnode_rec *fnode, struct iovec *iov)
{
	struct iscsi_flashnode_param_info *fnode_param;
	struct nlattr *attr;
	int len;
	uint8_t isid[6];

	len = sizeof(struct iscsi_flashnode_param_info) + 6;
	iov->iov_base = iscsi_nla_alloc(ISCSI_FLASHNODE_ISID, len);
	if (!iov->iov_base)
		return 1;

	attr = iov->iov_base;
	iov->iov_len = NLA_ALIGN(attr->nla_len);

	fnode_param = (struct iscsi_flashnode_param_info *)ISCSI_NLA_DATA(attr);
	fnode_param->param = ISCSI_FLASHNODE_ISID;
	fnode_param->len = 6;

	sscanf(fnode->sess.isid, "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx",
	       &isid[0], &isid[1], &isid[2], &isid[3], &isid[4], &isid[5]);

	memcpy(fnode_param->value, isid, fnode_param->len);
	return 0;
}

/*
 * Probe the controller in genetlink to find the family id
 * for the TASKSTATS family
 */
static int get_family_id(int sd)
{
	struct {
		struct nlmsghdr n;
		struct genlmsghdr g;
		char buf[256];
	} ans;

	int id = 0, rc;
	struct nlattr *na;
	int rep_len;

        char name[100];
	strcpy(name, TASKSTATS_GENL_NAME);
	rc = send_cmd (sd, GENL_ID_CTRL, getpid(), CTRL_CMD_GETFAMILY,
			CTRL_ATTR_FAMILY_NAME, (void *)name,
			strlen(TASKSTATS_GENL_NAME)+1);

	rep_len = recv(sd, &ans, sizeof(ans), 0);
	if (ans.n.nlmsg_type == NLMSG_ERROR ||
	    (rep_len < 0) || !NLMSG_OK((&ans.n), rep_len))
		return -1;

	na = (struct nlattr *) GENLMSG_DATA(&ans);
	na = (struct nlattr *) ((char *) na + NLA_ALIGN(na->nla_len));
	if (na->nla_type == CTRL_ATTR_FAMILY_ID) {
		id = *(__u16 *) NLA_DATA(na);
	}
	return id;
}

static void test_nfacct_del(struct netlink_info *netlink, const char *name)
{
	struct nfgenmsg *hdr;
	size_t len, name_len;

	name_len = strlen(name) + 1;
	len = NLMSG_ALIGN(sizeof(struct nfgenmsg)) +
		NLA_ALIGN(sizeof(struct nlattr)) +
		name_len;

	hdr = g_malloc0(len);

	hdr->nfgen_family = AF_UNSPEC;
	hdr->version = NFNETLINK_V0;
	hdr->res_id = 0;

	append_attr_str(NLA_DATA(hdr), NFACCT_NAME, name_len, name);

	netlink_send(netlink,
			NFNL_SUBSYS_ACCT << 8 | NFNL_MSG_ACCT_DEL,
			NLM_F_ACK, hdr, len,
			test_nfacct_callback, NULL, NULL);

	g_free(hdr);
}

/**
 * Return next attribute in a stream of attributes.
 * @arg nla		Attribute of any kind.
 * @arg remaining	Variable to count remaining bytes in stream.
 *
 * Calculates the offset to the next attribute based on the attribute
 * given. The attribute provided is assumed to be accessible, the
 * caller is responsible to use nla_ok() beforehand. The offset (length
 * of specified attribute including padding) is then subtracted from
 * the remaining bytes variable and a pointer to the next attribute is
 * returned.
 *
 * nla_next() can be called as long as remainig is >0.
 *
 * @return Pointer to next attribute.
 */
struct nlattr *nla_next(const struct nlattr *nla, int *remaining)
{
	int totlen = NLA_ALIGN(nla->nla_len);

	*remaining -= totlen;
	return (struct nlattr *) ((char *) nla + totlen);
}

/*
 * Probe the controller in genetlink to find the family id
 * for the TASKSTATS family
 */
static int get_family_id(xbt_cpu_timer_t timer)
{
	struct {
		struct nlmsghdr n;
		struct genlmsghdr g;
		char buf[256];
	} ans;

	int id = 0;
	struct nlattr *na;
	int rep_len;
	char name[100];
	int sd = timer->_nl_sd;


	strcpy(name, TASKSTATS_GENL_NAME);
	netlink_sock_send(GENL_ID_CTRL, CTRL_CMD_GETFAMILY,
			CTRL_ATTR_FAMILY_NAME, (void *) name, strlen(TASKSTATS_GENL_NAME) + 1, timer);

	rep_len = recv(sd, &ans, sizeof(ans), 0);
	xbt_assert(rep_len >= 0, "Answer to request on the family id is zero-sized");

	xbt_assert(ans.n.nlmsg_type != NLMSG_ERROR && NLMSG_OK((&ans.n), rep_len),
			"Error while retrieving the family id thru netlink: %s", strerror(errno));

	na = (struct nlattr *) GENLMSG_DATA(&ans);
	na = (struct nlattr *) ((char *) na + NLA_ALIGN(na->nla_len));
	if (na->nla_type == CTRL_ATTR_FAMILY_ID) {
		id = *(__u16 *) NLA_DATA(na);
	}
	return id;
}

/*
 * ---------------------------------------------------------------------------
 * Returns alligned length of the attribute.
 * ---------------------------------------------------------------------------
 */
UINT32
NlAttrLenPad(const PNL_ATTR nla, UINT32 maxlen)
{
    UINT32 len = NLA_ALIGN(nla->nlaLen);

    return len <= maxlen ? len : nla->nlaLen;
}

int
nl_build_attr_linkinfo(struct nl_client *cl, struct vn_if *ifp)
{
    char *link_info_buf;
    int len;
    struct nlattr *nla = (struct nlattr *)
        ((char *)cl->cl_buf + cl->cl_buf_offset);

    len = NLA_HDRLEN + NLA_HDRLEN +
        NLA_ALIGN(strlen(ifp->if_kind) + 1);

    if (cl->cl_buf_offset + len > cl->cl_buf_len)
        return -ENOMEM;

    nla->nla_len = len;
    nla->nla_type = IFLA_LINKINFO;

    link_info_buf = (char *)nla + NLA_HDRLEN;
    nla = (struct nlattr *)link_info_buf;
    nla->nla_len = len - NLA_HDRLEN;
    nla->nla_type = IFLA_INFO_KIND;

    link_info_buf += NLA_HDRLEN;
    strcpy(link_info_buf, ifp->if_kind);

    cl->cl_buf_offset += len;

    return 0;
}

int get_family_id(int sd)
{
	struct msgtemplate msg;
	int	len;
	int	recv_len;
	int	rc;
	struct nlattr *na;

	rc = send_cmd(sd, GENL_ID_CTRL, CTRL_CMD_GETFAMILY,
			CTRL_ATTR_FAMILY_NAME, SAMPLE_NETLINK_NL_FAMILY_NAME,
			strlen(SAMPLE_NETLINK_NL_FAMILY_NAME)+1, NLM_F_REQUEST);
	if (rc < 0) {
		printf("Error sending family cmd (%d:%s)\n",
				errno, strerror(errno));
		return -1;
	}
	recv_len = recv(sd, &msg, sizeof(msg), 0);
	if (msg.n.nlmsg_type == NLMSG_ERROR) {
		printf("Error: recv family error msg\n");
		return -1;
	}
	if (recv_len < 0) {
		printf("Error: recv family (%d)\n", recv_len);
		return -1;
	}
	if (!NLMSG_OK((&msg.n), recv_len)) {
		printf("Error: recv family msg nok\n");
		return -1;
	}

	len = 0;
	recv_len = GENLMSG_PAYLOAD(&msg.n);
	na = (struct nlattr *) GENLMSG_DATA(&msg);
	while (len < recv_len) {
		len += NLA_ALIGN(na->nla_len);
		switch (na->nla_type) {
			case CTRL_ATTR_FAMILY_ID:
				nl_family_id = *(uint16_t *) NLA_DATA(na);
				printf("family id:%d\n", nl_family_id);
				break;
			case CTRL_ATTR_MCAST_GROUPS:
				parse_groups(NLA_DATA(na),
						NLA_PAYLOAD_LEN(na->nla_len));
				break;
			case CTRL_ATTR_FAMILY_NAME:
			case CTRL_ATTR_VERSION:
			case CTRL_ATTR_HDRSIZE:
			case CTRL_ATTR_MAXATTR:
			case CTRL_ATTR_OPS:
				printf("Unused family attr %d\n", na->nla_type);
				break;
			default:
				printf("Unknown family attr %d\n", na->nla_type);
				break;
		}
		na = (struct nlattr *) (GENLMSG_DATA(&msg) + len);
	}

	return nl_family_id;
}

static inline int nla_put_string(struct nlattr *nla, int type, const char *str)
{
	int attrlen = strlen(str) + 1;

	nla->nla_len = NLA_HDRLEN + attrlen;
	nla->nla_type = type;
	memcpy(nla_data(nla), str, attrlen);

	return NLA_HDRLEN + NLA_ALIGN(attrlen);
}