1 #include <err.h>
2 #include <errno.h>
3 #include <linux/qrtr.h>
4 #include <linux/netlink.h>
5 #include <linux/rtnetlink.h>
6 #include <stdint.h>
7 #include <stdio.h>
8 #include <string.h>
9 #include <sys/socket.h>
10 #include <unistd.h>
11
12 #include "libqrtr.h"
13 #include "logging.h"
14
qrtr_set_address(uint32_t addr)15 void qrtr_set_address(uint32_t addr)
16 {
17 struct {
18 struct nlmsghdr nh;
19 struct ifaddrmsg ifa;
20 char attrbuf[32];
21 } req;
22 struct {
23 struct nlmsghdr nh;
24 struct nlmsgerr err;
25 } resp;
26 struct sockaddr_qrtr sq;
27 struct rtattr *rta;
28 socklen_t sl = sizeof(sq);
29 int sock;
30 int ret;
31
32 /* Trigger loading of the qrtr kernel module */
33 sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
34 if (sock < 0)
35 PLOGE_AND_EXIT("failed to create AF_QIPCRTR socket");
36
37 ret = getsockname(sock, (void*)&sq, &sl);
38 if (ret < 0)
39 PLOGE_AND_EXIT("getsockname()");
40 close(sock);
41
42 /* Skip configuring the address, if it's same as current */
43 if (sl == sizeof(sq) && sq.sq_node == addr)
44 return;
45
46 sock = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
47 if (sock < 0)
48 PLOGE_AND_EXIT("failed to create netlink socket");
49
50 memset(&req, 0, sizeof(req));
51 req.nh.nlmsg_len = NLMSG_SPACE(sizeof(struct ifaddrmsg));
52 req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
53 req.nh.nlmsg_type = RTM_NEWADDR;
54 req.ifa.ifa_family = AF_QIPCRTR;
55
56 rta = (struct rtattr *)(((char *) &req) + req.nh.nlmsg_len);
57 rta->rta_type = IFA_LOCAL;
58 rta->rta_len = RTA_LENGTH(sizeof(addr));
59 memcpy(RTA_DATA(rta), &addr, sizeof(addr));
60
61 req.nh.nlmsg_len += rta->rta_len;
62
63 ret = send(sock, &req, req.nh.nlmsg_len, 0);
64 if (ret < 0)
65 PLOGE_AND_EXIT("failed to send netlink request");
66
67 ret = recv(sock, &resp, sizeof(resp), 0);
68 if (ret < 0)
69 PLOGE_AND_EXIT("failed to receive netlink response");
70
71 if (resp.nh.nlmsg_type == NLMSG_ERROR && resp.err.error != 0) {
72 errno = -resp.err.error;
73 PLOGE_AND_EXIT("failed to configure node id");
74 }
75
76 close(sock);
77 }
78