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 int get_pfc_cfg(char *ifname, __u8 *pfc)
{
struct nlmsghdr *nlh;
struct dcbmsg *d;
struct rtattr *rta_parent, *rta_child;
int i;
nlh = start_msg(RTM_GETDCB, DCB_CMD_PFC_GCFG);
if (NULL==nlh)
return -EIO;
add_rta(nlh, DCB_ATTR_IFNAME, (void *)ifname,
strlen(ifname) + 1);
rta_parent = add_rta(nlh, DCB_ATTR_PFC_CFG, NULL, 0);
rta_child = add_rta(nlh, DCB_PFC_UP_ATTR_ALL, NULL, 0);
rta_parent->rta_len += NLMSG_ALIGN(rta_child->rta_len);
if (send_msg(nlh))
return -EIO;
nlh = get_msg();
if (!nlh)
return -EIO;
d = (struct dcbmsg *)NLMSG_DATA(nlh);
rta_parent = (struct rtattr *)(((char *)d) +
NLMSG_ALIGN(sizeof(struct dcbmsg)));
if (d->cmd != DCB_CMD_PFC_GCFG) {
printf("Hmm, this is not the message we were expecting.\n");
return -EIO;
}
if (rta_parent->rta_type != DCB_ATTR_PFC_CFG) {
/* Do we really want to code up an attribute parser?? */
printf("A full libnetlink (with genl and attribute support) "
"would sure be nice.\n");
return -EIO;
}
rta_child = NLA_DATA(rta_parent);
rta_parent = (struct rtattr *)((char *)rta_parent +
NLMSG_ALIGN(rta_parent->rta_len));
for (i = 0; rta_parent > rta_child; i++) {
if (i == 8) {
printf("pfc array out of range\n");
break;
}
pfc[rta_child->rta_type - DCB_PFC_UP_ATTR_0] =
*(__u8 *)NLA_DATA(rta_child);
rta_child = (struct rtattr *)((char *)rta_child +
NLMSG_ALIGN(rta_child->rta_len));
}
if (rta_parent != rta_child)
printf("rta pointers are off\n");
return 0;
}
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);
}
}
int send_security_response(int sd, __u16 nlmsg_type, __u8 genl_cmd,
uint16_t flags, void *sec_flag, void *query_id)
{
struct nlattr *na;
struct sockaddr_nl nladdr;
int r, buflen;
char *buf;
uint32_t attr_len = 0;
struct msgtemplate msg;
msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
msg.n.nlmsg_type = nlmsg_type;
msg.n.nlmsg_flags = flags;
msg.n.nlmsg_seq = 0;
msg.n.nlmsg_pid = pid;
msg.g.cmd = genl_cmd;
msg.g.version = 0x1;
na = (struct nlattr *) GENLMSG_DATA(&msg);
na->nla_type = SAMPLE_NETLINK_ATTR_SEC_QID;
na->nla_len = sizeof(int) + NLA_HDRLEN;
memcpy(NLA_DATA(na), query_id, sizeof(int));
attr_len += na->nla_len;
na = (struct nlattr *) (GENLMSG_DATA(&msg) + attr_len);
na->nla_type = SAMPLE_NETLINK_ATTR_SECURITY_FLAG;
na->nla_len = sizeof(int) + NLA_HDRLEN;
memcpy(NLA_DATA(na), sec_flag, sizeof(int));
attr_len += na->nla_len;
msg.n.nlmsg_len += NLMSG_ALIGN(attr_len);
buf = (char *) &msg;
buflen = msg.n.nlmsg_len ;
memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
while ((r = sendto(sd, buf, buflen, 0, (struct sockaddr *) &nladdr,
sizeof(nladdr))) < buflen) {
printf("sending (r=%d)\n", r);
if (r > 0) {
buf += r;
buflen -= r;
} else if (errno != EAGAIN)
return r;
}
printf("msg sent (r=%d)\n", r);
return 0;
}
/*
* Probe the controller in genetlink to find the family id
*/
int ResetManager::get_family_id(int sk, const char *family_name)
{
struct nlattr *na;
int rep_len;
int id = -1;
GENERIC_NETLINK_PACKET family_req, ans;
/* Get family name */
family_req.n.nlmsg_type = GENL_ID_CTRL;
family_req.n.nlmsg_flags = NLM_F_REQUEST;
family_req.n.nlmsg_seq = 0;
family_req.n.nlmsg_pid = getpid();
family_req.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
family_req.g.cmd = CTRL_CMD_GETFAMILY;
family_req.g.version = 0x1;
na = (struct nlattr *) GENLMSG_DATA(&family_req);
na->nla_type = CTRL_ATTR_FAMILY_NAME;
na->nla_len = strlen(family_name) + 1 + NLA_HDRLEN;
strcpy((char *)NLA_DATA(na), family_name);
family_req.n.nlmsg_len += NLMSG_ALIGN(na->nla_len);
if (sendto_fd(sk, (char *) &family_req, family_req.n.nlmsg_len) < 0) {
return -1;
}
rep_len = recv(sk, &ans, sizeof(ans), 0);
if (rep_len < 0){
LOGE("no response\n");
return -1;
}
/* Validate response message */
else if (!NLMSG_OK((&ans.n), (unsigned int)rep_len)){
LOGE("invalid reply message\n");
return -1;
}
else if (ans.n.nlmsg_type == NLMSG_ERROR) { /* error */
LOGE("received error\n");
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 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;
}
/*
* 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;
}
static int get_pfc_state(char *ifname, __u8 *state)
{
struct nlmsghdr *nlh;
struct dcbmsg *d;
struct rtattr *rta;
nlh = start_msg(RTM_GETDCB, DCB_CMD_PFC_GSTATE);
if (NULL==nlh)
return -EIO;
add_rta(nlh, DCB_ATTR_IFNAME, (void *)ifname, strlen(ifname) + 1);
if (send_msg(nlh))
return -EIO;
free(nlh);
nlh = get_msg();
if (!nlh)
return -EIO;
d = (struct dcbmsg *)NLMSG_DATA(nlh);
rta = (struct rtattr *)(((char *)d) +
NLMSG_ALIGN(sizeof(struct dcbmsg)));
if (d->cmd != DCB_CMD_PFC_GSTATE) {
printf("Hmm, this is not the message we were expecting.\n");
return -EIO;
}
if (rta->rta_type != DCB_ATTR_PFC_STATE) {
return -EIO;
}
*state = *(__u8 *)NLA_DATA(rta);
return 0;
}
static int recv_msg(int cmd, int attr)
{
struct nlmsghdr *nlh;
struct dcbmsg *d;
struct rtattr *rta;
int rval;
nlh = get_msg();
if (NULL == nlh)
return -EIO;
d = (struct dcbmsg *)NLMSG_DATA(nlh);
rta = (struct rtattr *)(((char *)d) +
NLMSG_ALIGN(sizeof(struct dcbmsg)));
if ((d->cmd != cmd) || (rta->rta_type != attr)) {
printf("Bad netlink message attribute.");
return -EIO;
}
rval = *(__u8 *)NLA_DATA(rta);
free(nlh);
return rval;
}
void process_netlink(int ci)
{
struct msgtemplate msg;
struct nlattr *na;
int len;
int fd;
fd = client_fd(ci);
len = recv(fd, &msg, sizeof(msg), 0);
if (len < 0) {
log_error("nonfatal netlink error: errno %d", errno);
return;
}
if (msg.n.nlmsg_type == NLMSG_ERROR || !NLMSG_OK((&msg.n), len)) {
struct nlmsgerr *err = NLMSG_DATA(&msg);
log_error("fatal netlink error: errno %d", err->error);
return;
}
na = (struct nlattr *) GENLMSG_DATA(&msg);
process_timewarn((struct dlm_lock_data *) NLA_DATA(na));
}
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);
}
/*
* 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 int get_perm_hwaddr(char *ifname, __u8 *buf_perm, __u8 *buf_san)
{
struct nlmsghdr *nlh;
struct dcbmsg *d;
struct rtattr *rta;
nlh = start_msg(RTM_GETDCB, DCB_CMD_GPERM_HWADDR);
if (NULL==nlh)
return -EIO;
add_rta(nlh, DCB_ATTR_IFNAME, (void *)ifname, strlen(ifname) + 1);
add_rta(nlh, DCB_ATTR_PERM_HWADDR, NULL, 0);
if (send_msg(nlh))
return -EIO;
nlh = get_msg();
if (!nlh) {
printf("get msg failed\n");
return -EIO;
}
d = (struct dcbmsg *)NLMSG_DATA(nlh);
rta = (struct rtattr *)(((char *)d) +
NLMSG_ALIGN(sizeof(struct dcbmsg)));
if (d->cmd != DCB_CMD_GPERM_HWADDR) {
printf("Hmm, this is not the message we were expecting.\n");
return -EIO;
}
if (rta->rta_type != DCB_ATTR_PERM_HWADDR) {
/* Do we really want to code up an attribute parser?? */
printf("A full libnetlink (with genl and attribute support) "
"would sure be nice.\n");
return -EIO;
}
memcpy(buf_perm, NLA_DATA(rta), ETH_ALEN);
memcpy(buf_san, NLA_DATA(rta + ETH_ALEN*sizeof(__u8)), ETH_ALEN);
return 0;
}
struct nl_response *
nl_parse_gen_ctrl(struct nl_client *cl)
{
int len;
struct nlattr *nla;
struct genl_ctrl_message *msg;
char *buf = cl->cl_buf + cl->cl_buf_offset;
struct nl_response *resp = &cl->resp;
msg = (struct genl_ctrl_message *)cl->cl_resp_buf;
resp->nl_data = (uint8_t *)(msg);
memset(msg, 0, sizeof(*msg));
msg->family_id = -1;
len = cl->cl_msg_len - (cl->cl_buf_offset - cl->cl_msg_start);
nla = (struct nlattr *)buf;
while (len > 0 && len > NLA_HDRLEN) {
if (len < NLA_ALIGN(nla->nla_len))
return nl_set_resp_err(cl, -EINVAL);
switch (nla->nla_type) {
case CTRL_ATTR_FAMILY_NAME:
strncpy(msg->family_name, NLA_DATA(nla), sizeof(msg->family_name) - 1);
break;
case CTRL_ATTR_FAMILY_ID:
msg->family_id = *(unsigned short *)NLA_DATA(nla);
break;
default:
break;
}
cl->cl_buf_offset += NLA_ALIGN(nla->nla_len);
len -= NLA_ALIGN(nla->nla_len);
nla = (struct nlattr *)((char *)nla + NLA_ALIGN(nla->nla_len));
}
return resp;
}
static void append_attr_str(struct nlattr *attr,
uint16_t type, size_t size, const char *str)
{
char *dst;
attr->nla_len = NLA_HDRLEN + size;
attr->nla_type = NFACCT_NAME;
dst = (char *)NLA_DATA(attr);
strncpy(dst, str, size);
dst[size - 1] = '\0';
}
static void test_nfacct_dump_callback(unsigned int error, uint16_t type,
const void *data, uint32_t len, void *user_data)
{
const struct nfgenmsg *msg = data;
const struct nlattr *attr;
uint64_t packets = 0 , bytes = 0;
char *name = NULL;
int attrlen;
if (error == EINVAL)
printf("nfnetlink_acct not loaded\n");
g_assert_cmpuint(error, ==, 0);
attrlen = len - NLMSG_ALIGN(sizeof(struct nfgenmsg));
for (attr = NFGEN_DATA(msg); NLA_OK(attr, attrlen);
attr = NLA_NEXT(attr, attrlen)) {
switch (attr->nla_type) {
case NFACCT_NAME:
name = NLA_DATA(attr);
break;
case NFACCT_PKTS:
packets = be64toh(*(uint64_t *) NLA_DATA(attr));
break;
case NFACCT_BYTES:
bytes = be64toh(*(uint64_t *) NLA_DATA(attr));
break;
case NFACCT_USE:
break;
}
}
printf("%s packets %" PRIu64 " bytes %" PRIu64 "\n",
name, packets, bytes);
g_main_loop_quit(mainloop);
}
static struct rtattr *add_rta(struct nlmsghdr *nlh, __u16 rta_type,
void *attr, __u16 rta_len)
{
struct rtattr *rta;
rta = (struct rtattr *)((char *)nlh + nlh->nlmsg_len);
rta->rta_type = rta_type;
rta->rta_len = rta_len + NLA_HDRLEN;
if (attr)
memcpy(NLA_DATA(rta), attr, rta_len);
nlh->nlmsg_len += NLMSG_ALIGN(rta->rta_len);
return rta;
}
static int receive_msg(u_int16_t fid, int sock, struct packet *data)
{
int ret;
nlmsg msg;
struct nlattr *na;
ret = recv(sock, &msg, sizeof(msg), 0);
if (ret < 0 || msg.n.nlmsg_type == NLMSG_ERROR ||
msg.n.nlmsg_type != fid || fid == 0)
return -1;
na = (struct nlattr *) GENLMSG_DATA(&msg);
*data = *(struct packet *) NLA_DATA(na);
return 0;
}
/*
* Netlink interface
*/
int send_cmd(int sd, __u16 nlmsg_type, __u8 genl_cmd, __u16 nla_type,
void *nla_data, int nla_len, uint16_t flags)
{
struct nlattr *na;
struct sockaddr_nl nladdr;
int r, buflen;
char *buf;
struct msgtemplate msg;
msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
msg.n.nlmsg_type = nlmsg_type;
msg.n.nlmsg_flags = flags;
msg.n.nlmsg_seq = 0;
msg.n.nlmsg_pid = pid;
msg.g.cmd = genl_cmd;
msg.g.version = 0x1;
na = (struct nlattr *) GENLMSG_DATA(&msg);
na->nla_type = nla_type;
na->nla_len = nla_len + NLA_HDRLEN;
memcpy(NLA_DATA(na), nla_data, nla_len);
msg.n.nlmsg_len += NLMSG_ALIGN(na->nla_len);
buf = (char *) &msg;
buflen = msg.n.nlmsg_len ;
memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
while ((r = sendto(sd, buf, buflen, 0, (struct sockaddr *) &nladdr,
sizeof(nladdr))) < buflen) {
printf("sending (r=%d)\n", r);
if (r > 0) {
buf += r;
buflen -= r;
} else if (errno != EAGAIN)
return r;
}
printf("msg sent (r=%d)\n", r);
return 0;
}
static int
nla_put_32(struct nlmsghdr *n, unsigned short maxlen,
unsigned short type, uint32_t data)
{
unsigned short len;
struct nlattr *nla;
len = NLA_ALIGN(NLA_HDRLEN + sizeof(data));
if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
errno = ENOBUFS;
return -1;
}
nla = (struct nlattr *)NLMSG_TAIL(n);
nla->nla_type = type;
nla->nla_len = len;
memcpy(NLA_DATA(nla), &data, sizeof(data));
n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + len;
return 0;
}
static int
nla_put_string(struct nlmsghdr *n, unsigned short maxlen,
unsigned short type, const char *data)
{
struct nlattr *nla;
size_t len, sl;
sl = strlen(data) + 1;
len = NLA_ALIGN(NLA_HDRLEN + sl);
if (NLMSG_ALIGN(n->nlmsg_len) + len > maxlen) {
errno = ENOBUFS;
return -1;
}
nla = (struct nlattr *)NLMSG_TAIL(n);
nla->nla_type = type;
nla->nla_len = (unsigned short)len;
memcpy(NLA_DATA(nla), data, sl);
n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + (unsigned short)len;
return 0;
}
static int send_genetlink_cmd(int sd, uint16_t nlmsg_type, uint32_t nlmsg_pid,
uint8_t genl_cmd, uint16_t nla_type,
void *nla_data, int nla_len)
{
struct nlattr *na;
struct sockaddr_nl nladdr;
int r, buflen;
char *buf;
struct msgtemplate msg;
msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
msg.n.nlmsg_type = nlmsg_type;
msg.n.nlmsg_flags = NLM_F_REQUEST;
msg.n.nlmsg_seq = 0;
msg.n.nlmsg_pid = nlmsg_pid;
msg.g.cmd = genl_cmd;
msg.g.version = 0x1;
na = (struct nlattr *) GENLMSG_DATA(&msg);
na->nla_type = nla_type;
na->nla_len = nla_len + 1 + NLA_HDRLEN;
if (nla_data)
memcpy(NLA_DATA(na), nla_data, nla_len);
msg.n.nlmsg_len += NLMSG_ALIGN(na->nla_len);
buf = (char *) &msg;
buflen = msg.n.nlmsg_len ;
memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
while ((r = sendto(sd, buf, buflen, 0, (struct sockaddr *) &nladdr,
sizeof(nladdr))) < buflen) {
if (r > 0) {
buf += r;
buflen -= r;
} else if (errno != EAGAIN)
return -1;
}
return 0;
}
static int get_state(char *ifname, __u8 *state)
{
struct nlmsghdr *nlh;
struct dcbmsg *d;
struct rtattr *rta;
nlh = start_msg(RTM_GETDCB, DCB_CMD_GSTATE);
if (NULL==nlh)
return -EIO;
add_rta(nlh, DCB_ATTR_IFNAME, (void *)ifname, strlen(ifname) + 1);
if (send_msg(nlh))
return -EIO;
free(nlh);
nlh = get_msg();
if (!nlh)
return -EIO;
d = (struct dcbmsg *)NLMSG_DATA(nlh);
rta = (struct rtattr *)(((char *)d) +
NLMSG_ALIGN(sizeof(struct dcbmsg)));
if (d->cmd != DCB_CMD_GSTATE) {
printf("Hmm, this is not the message we were expecting.\n");
return -EIO;
}
if (rta->rta_type != DCB_ATTR_STATE) {
/* Do we really want to code up an attribute parser?? */
printf("A full libnetlink (with genl and attribute support) "
"would sure be nice.\n");
return -EIO;
}
*state = *(__u8 *)NLA_DATA(rta);
return 0;
}
static int get_fid(int fd, char *fname)
{
nlmsg ans;
int id = 0, rc;
struct nlattr *na;
int rep_len;
rc = send_msg(fd, GENL_ID_CTRL, 0, CTRL_CMD_GETFAMILY, 1, CTRL_ATTR_FAMILY_NAME, (void *)fname, strlen(fname) + 1);
if (rc < 0)
goto ret;
rep_len = recv(fd, &ans, sizeof(ans), 0);
//if (rep_len < 0 || ans.n.nlmsg_type == NLMSG_ERROR || !NLMSG_OK((&ans.n), rep_len))
if (rep_len < 0 || ans.n.nlmsg_type == NLMSG_ERROR)
goto ret;
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);
ret:
return id;
}
static void netlink_sock_send(__u16 nlmsg_type, __u8 genl_cmd, __u16 nla_type, void *nla_data,
int nla_len, xbt_cpu_timer_t timer)
{
struct nlattr *na;
struct sockaddr_nl nladdr;
int r, buflen;
char *buf;
struct msgtemplate msg = timer->msg;
int sd = timer->_nl_sd;
__u32 nlmsg_pid = timer->pid;
msg.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
msg.n.nlmsg_type = nlmsg_type;
msg.n.nlmsg_flags = NLM_F_REQUEST;
msg.n.nlmsg_seq = 0;
msg.n.nlmsg_pid = nlmsg_pid;
msg.g.cmd = genl_cmd;
msg.g.version = 0x1;
na = (struct nlattr *) GENLMSG_DATA(&msg);
na->nla_type = nla_type;
na->nla_len = nla_len + 1 + NLA_HDRLEN;
memcpy(NLA_DATA(na), nla_data, nla_len);
msg.n.nlmsg_len += NLMSG_ALIGN(na->nla_len);
buf = (char *) &msg;
buflen = msg.n.nlmsg_len;
memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
while ((r = sendto(sd, buf, buflen, 0, (struct sockaddr *) &nladdr, sizeof(nladdr))) < buflen) {
if (r > 0) {
buf += r;
buflen -= r;
} else if (errno != EAGAIN)
xbt_die("Error while sending a command over netlink: %s", strerror(errno));
}
}
int main(int argc, char *argv[])
{
int c, rc, rep_len, aggr_len, len2;
int cmd_type = TASKSTATS_CMD_ATTR_UNSPEC;
__u16 id;
__u32 mypid;
struct nlattr *na;
int nl_sd = -1;
int len = 0;
pid_t tid = 0;
pid_t rtid = 0;
int fd = 0;
int count = 0;
int write_file = 0;
int maskset = 0;
char *logfile = NULL;
int loop = 0;
int containerset = 0;
char *containerpath = NULL;
int cfd = 0;
int forking = 0;
sigset_t sigset;
struct msgtemplate msg;
while (!forking) {
c = getopt(argc, argv, "qdiw:r:m:t:p:vlC:c:");
if (c < 0)
break;
switch (c) {
case 'd':
printf("print delayacct stats ON\n");
print_delays = 1;
break;
case 'i':
printf("printing IO accounting\n");
print_io_accounting = 1;
break;
case 'q':
printf("printing task/process context switch rates\n");
print_task_context_switch_counts = 1;
break;
case 'C':
containerset = 1;
containerpath = optarg;
break;
case 'w':
logfile = strdup(optarg);
printf("write to file %s\n", logfile);
write_file = 1;
break;
case 'r':
rcvbufsz = atoi(optarg);
printf("receive buf size %d\n", rcvbufsz);
if (rcvbufsz < 0)
err(1, "Invalid rcv buf size\n");
break;
case 'm':
strncpy(cpumask, optarg, sizeof(cpumask));
cpumask[sizeof(cpumask) - 1] = '\0';
maskset = 1;
printf("cpumask %s maskset %d\n", cpumask, maskset);
break;
case 't':
tid = atoi(optarg);
if (!tid)
err(1, "Invalid tgid\n");
cmd_type = TASKSTATS_CMD_ATTR_TGID;
break;
case 'p':
tid = atoi(optarg);
if (!tid)
err(1, "Invalid pid\n");
cmd_type = TASKSTATS_CMD_ATTR_PID;
break;
case 'c':
/* Block SIGCHLD for sigwait() later */
if (sigemptyset(&sigset) == -1)
err(1, "Failed to empty sigset");
if (sigaddset(&sigset, SIGCHLD))
err(1, "Failed to set sigchld in sigset");
sigprocmask(SIG_BLOCK, &sigset, NULL);
/* fork/exec a child */
tid = fork();
if (tid < 0)
err(1, "Fork failed\n");
if (tid == 0)
if (execvp(argv[optind - 1],
&argv[optind - 1]) < 0)
exit(-1);
/* Set the command type and avoid further processing */
cmd_type = TASKSTATS_CMD_ATTR_PID;
forking = 1;
break;
case 'v':
printf("debug on\n");
dbg = 1;
break;
case 'l':
printf("listen forever\n");
loop = 1;
break;
default:
usage();
exit(-1);
}
}
if (write_file) {
fd = open(logfile, O_WRONLY | O_CREAT | O_TRUNC,
S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
if (fd == -1) {
perror("Cannot open output file\n");
exit(1);
}
}
nl_sd = create_nl_socket(NETLINK_GENERIC);
if (nl_sd < 0)
err(1, "error creating Netlink socket\n");
mypid = getpid();
id = get_family_id(nl_sd);
if (!id) {
fprintf(stderr, "Error getting family id, errno %d\n", errno);
goto err;
}
PRINTF("family id %d\n", id);
if (maskset) {
rc = send_cmd(nl_sd, id, mypid, TASKSTATS_CMD_GET,
TASKSTATS_CMD_ATTR_REGISTER_CPUMASK,
&cpumask, strlen(cpumask) + 1);
PRINTF("Sent register cpumask, retval %d\n", rc);
if (rc < 0) {
fprintf(stderr, "error sending register cpumask\n");
goto err;
}
}
if (tid && containerset) {
fprintf(stderr, "Select either -t or -C, not both\n");
goto err;
}
/*
* If we forked a child, wait for it to exit. Cannot use waitpid()
* as all the delicious data would be reaped as part of the wait
*/
if (tid && forking) {
int sig_received;
sigwait(&sigset, &sig_received);
}
if (tid) {
rc = send_cmd(nl_sd, id, mypid, TASKSTATS_CMD_GET,
cmd_type, &tid, sizeof(__u32));
PRINTF("Sent pid/tgid, retval %d\n", rc);
if (rc < 0) {
fprintf(stderr, "error sending tid/tgid cmd\n");
goto done;
}
}
if (containerset) {
cfd = open(containerpath, O_RDONLY);
if (cfd < 0) {
perror("error opening container file");
goto err;
}
rc = send_cmd(nl_sd, id, mypid, CGROUPSTATS_CMD_GET,
CGROUPSTATS_CMD_ATTR_FD, &cfd, sizeof(__u32));
if (rc < 0) {
perror("error sending cgroupstats command");
goto err;
}
}
if (!maskset && !tid && !containerset) {
usage();
goto err;
}
do {
rep_len = recv(nl_sd, &msg, sizeof(msg), 0);
PRINTF("received %d bytes\n", rep_len);
if (rep_len < 0) {
fprintf(stderr, "nonfatal reply error: errno %d\n",
errno);
continue;
}
if (msg.n.nlmsg_type == NLMSG_ERROR ||
!NLMSG_OK((&msg.n), rep_len)) {
struct nlmsgerr *err = NLMSG_DATA(&msg);
fprintf(stderr, "fatal reply error, errno %d\n",
err->error);
goto done;
}
PRINTF("nlmsghdr size=%zu, nlmsg_len=%d, rep_len=%d\n",
sizeof(struct nlmsghdr), msg.n.nlmsg_len, rep_len);
rep_len = GENLMSG_PAYLOAD(&msg.n);
na = (struct nlattr *) GENLMSG_DATA(&msg);
len = 0;
while (len < rep_len) {
len += NLA_ALIGN(na->nla_len);
switch (na->nla_type) {
case TASKSTATS_TYPE_AGGR_TGID:
/* Fall through */
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);
done = 0;
while (len2 < aggr_len) {
switch (na->nla_type) {
case TASKSTATS_TYPE_PID:
rtid = *(int *) NLA_DATA(na);
if (print_delays)
printf("PID\t%d\n", rtid);
break;
case TASKSTATS_TYPE_TGID:
rtid = *(int *) NLA_DATA(na);
if (print_delays)
printf("TGID\t%d\n", rtid);
break;
case TASKSTATS_TYPE_STATS:
count++;
if (print_delays)
print_delayacct((struct taskstats *) NLA_DATA(na));
if (print_io_accounting)
print_ioacct((struct taskstats *) NLA_DATA(na));
if (print_task_context_switch_counts)
task_context_switch_counts((struct taskstats *) NLA_DATA(na));
if (fd) {
if (write(fd, NLA_DATA(na), na->nla_len) < 0) {
err(1,"write error\n");
}
}
if (!loop)
goto done;
break;
case TASKSTATS_TYPE_NULL:
break;
default:
fprintf(stderr, "Unknown nested"
" nla_type %d\n",
na->nla_type);
break;
}
len2 += NLA_ALIGN(na->nla_len);
na = (struct nlattr *)((char *)na +
NLA_ALIGN(na->nla_len));
}
break;
case CGROUPSTATS_TYPE_CGROUP_STATS:
print_cgroupstats(NLA_DATA(na));
break;
default:
fprintf(stderr, "Unknown nla_type %d\n",
na->nla_type);
case TASKSTATS_TYPE_NULL:
break;
}
na = (struct nlattr *) (GENLMSG_DATA(&msg) + len);
}
} while (loop);
done:
if (maskset) {
rc = send_cmd(nl_sd, id, mypid, TASKSTATS_CMD_GET,
TASKSTATS_CMD_ATTR_DEREGISTER_CPUMASK,
&cpumask, strlen(cpumask) + 1);
printf("Sent deregister mask, retval %d\n", rc);
if (rc < 0)
err(rc, "error sending deregister cpumask\n");
}
err:
close(nl_sd);
if (fd)
close(fd);
if (cfd)
close(cfd);
return 0;
}
int ResetManager::start() {
struct sockaddr_nl nladdr;
int sz = 64 * 1024;
GENERIC_NETLINK_PACKET ans, req;
struct nlattr *na;
int id;
int mlength;
const char *message = "HELLO"; //message
int rc;
int count = 10;
if(mHandler) {
LOGD("ResetManager is already started");
return 0;
}
mlength = strlen(message) + 1;
LOGD("Size of initial message: %d ", mlength);
memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
nladdr.nl_pid = getpid();
nladdr.nl_groups = 0xffffffff;
if ((mSock = socket(AF_NETLINK,
SOCK_RAW,NETLINK_GENERIC)) < 0) {
LOGE("Unable to create uevent socket: %s", strerror(errno));
return -1;
}
if (setsockopt(mSock, SOL_SOCKET, SO_RCVBUFFORCE, &sz, sizeof(sz)) < 0) {
LOGE("Unable to set uevent socket options: %s", strerror(errno));
return -1;
}
if (bind(mSock, (struct sockaddr *) &nladdr, sizeof(nladdr)) < 0) {
LOGE("Unable to bind uevent socket: %s", strerror(errno));
return -1;
}
while(count--) {
/* 2. get family ID */
id = get_family_id(mSock, "MTK_WIFI");
if (-1 == id) {
LOGE("Unable to get family id, Retry");
usleep(500000);
} else {
LOGD("[MTK_WIFI] family id = %d\n", id);
break;
}
}
/* 2.9 Check WiFi Driver Support Chip Reset*/
if(id < 0) {
close(mSock);
mSock = -1;
LOGW("WiFi Driver Cannot Support Chip Reset, Abort ResetManager");
return -1;
}
/* 3. Prepare Dummy command */
req.n.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
req.n.nlmsg_type = id;
req.n.nlmsg_flags = NLM_F_REQUEST;
req.n.nlmsg_seq = 60;
req.n.nlmsg_pid = getpid();
req.g.cmd = 1; //MTK_WIFI_COMMAND_BIND
na = (struct nlattr *) GENLMSG_DATA(&req);
na->nla_type = 1; //MTK_WIFI_ATTR_MSG
na->nla_len = mlength + NLA_HDRLEN; //message length
memcpy(NLA_DATA(na), message, mlength);
req.n.nlmsg_len += NLMSG_ALIGN(na->nla_len);
LOGD("Reset Manager Prepare Dummy command");
/* 3.1 Send Command for binding */
memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
rc = sendto(mSock, (char *)&req, req.n.nlmsg_len, 0,
(struct sockaddr *) &nladdr, sizeof(nladdr));
LOGD("Reset Manager Send Command for binding");
mHandler = new ResetHandler(mCl, mSock);
if (mHandler->start()) {
LOGE("Unable to start ResetHandler: %s", strerror(errno));
return -1;
}
return 0;
}
static void receive_taskstats(void)
{
int rep_len, len2, aggr_len;
int len = 0, count = 0, resolved = 0;
struct msgtemplate msg;
struct nlattr *na;
pid_t rtid = 0;
do {
DEBUG("record: %d ", count);
rep_len = recv(nl_fd, &msg, sizeof(msg), 0);
if (rep_len < 0) {
fprintf(stderr, "nonfatal reply error: errno %d\n", errno);
continue;
}
if (msg.n.nlmsg_type == NLMSG_ERROR || !NLMSG_OK((&msg.n), rep_len)) {
struct nlmsgerr *err = NLMSG_DATA(&msg);
fprintf(stderr, "fatal reply error, errno %d\n", err->error);
return;
}
DEBUG("nlmsghdr size=%zu, nlmsg_len=%d, rep_len=%d\n",
sizeof(struct nlmsghdr), msg.n.nlmsg_len, rep_len);
rep_len = GENLMSG_PAYLOAD(&msg.n);
na = (struct nlattr *) GENLMSG_DATA(&msg);
len = 0;
while (len < rep_len) {
len += NLA_ALIGN(na->nla_len);
switch (na->nla_type) {
case TASKSTATS_TYPE_NULL:
break;
case TASKSTATS_TYPE_AGGR_TGID:
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:
rtid = *(int *) NLA_DATA(na);
if (rtid == current_query)
resolved = 1;
DEBUG("receive: rtid PID\t%d\n", rtid);
break;
case TASKSTATS_TYPE_TGID:
rtid = *(int *) NLA_DATA(na);
if (rtid == current_query)
resolved = 1;
fprintf(stderr, "rtid TGID\t%d\n", rtid);
break;
case TASKSTATS_TYPE_STATS:
count++;
if (rtid == current_query)
gather_data((struct taskstats *) NLA_DATA(na));
else
handle_async_event((struct taskstats *) NLA_DATA(na), rtid);
break;
default:
fprintf(stderr, "Unknown nested nla_type %d\n",
na->nla_type);
break;
}
len2 += NLA_ALIGN(na->nla_len);
na = (struct nlattr *) ((char *) na + len2);
}
break;
default:
fprintf(stderr, "Unknown nla_type %d\n", na->nla_type);
}
na = (struct nlattr *) (GENLMSG_DATA(&msg) + len);
}
} while (!resolved);
}
static int genetlink_resolve_family(const char *family)
{
struct nl_handler handler;
struct nlattr *attr;
struct genlmsg *request, *reply;
struct genlmsghdr *genlmsghdr;
int len, ret;
request = genlmsg_alloc(GENLMSG_GOOD_SIZE);
if (!request)
return -ENOMEM;
reply = genlmsg_alloc(GENLMSG_GOOD_SIZE);
if (!reply) {
genlmsg_free(request);
return -ENOMEM;
}
request->nlmsghdr.nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
request->nlmsghdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
request->nlmsghdr.nlmsg_type = GENL_ID_CTRL;
genlmsghdr = NLMSG_DATA(&request->nlmsghdr);
genlmsghdr->cmd = CTRL_CMD_GETFAMILY;
ret = netlink_open(&handler, NETLINK_GENERIC);
if (ret)
goto out;
ret = nla_put_string((struct nlmsg *)&request->nlmsghdr,
CTRL_ATTR_FAMILY_NAME, family);
if (ret)
goto out_close;
ret = netlink_transaction(&handler, (struct nlmsg *)&request->nlmsghdr,
(struct nlmsg *)&reply->nlmsghdr);
if (ret < 0)
goto out_close;
genlmsghdr = NLMSG_DATA(&reply->nlmsghdr);
len = reply->nlmsghdr.nlmsg_len;
ret = -ENOMSG;
if (reply->nlmsghdr.nlmsg_type != GENL_ID_CTRL)
goto out_close;
if (genlmsghdr->cmd != CTRL_CMD_NEWFAMILY)
goto out_close;
ret = -EMSGSIZE;
len -= NLMSG_LENGTH(GENL_HDRLEN);
if (len < 0)
goto out_close;
attr = (struct nlattr *)GENLMSG_DATA(reply);
attr = (struct nlattr *)((char *)attr + NLA_ALIGN(attr->nla_len));
ret = -ENOMSG;
if (attr->nla_type != CTRL_ATTR_FAMILY_ID)
goto out_close;
ret = *(__u16 *) NLA_DATA(attr);
out_close:
netlink_close(&handler);
out:
genlmsg_free(request);
genlmsg_free(reply);
return ret;
}
