1 #include <sys/types.h>
2 #include <sys/socket.h>
3 #include <sys/time.h>
4 #include <linux/qrtr.h>
5 #include <unistd.h>
6 #include <stdlib.h>
7 #include <string.h>
8 #include <stdio.h>
9 #include <errno.h>
10 #include <poll.h>
11
12 #include "libqrtr.h"
13 #include "logging.h"
14 #include "ns.h"
15
qrtr_getname(int sock,struct sockaddr_qrtr * sq)16 static int qrtr_getname(int sock, struct sockaddr_qrtr *sq)
17 {
18 socklen_t sl = sizeof(*sq);
19 int rc;
20
21 rc = getsockname(sock, (void *)sq, &sl);
22 if (rc) {
23 PLOGE("getsockname()");
24 return -1;
25 }
26
27 if (sq->sq_family != AF_QIPCRTR || sl != sizeof(*sq))
28 return -1;
29
30 return 0;
31 }
32
qrtr_open(int rport)33 int qrtr_open(int rport)
34 {
35 struct timeval tv;
36 int sock;
37 int rc;
38
39 sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
40 if (sock < 0) {
41 PLOGE("socket(AF_QIPCRTR)");
42 return -1;
43 }
44
45 tv.tv_sec = 1;
46 tv.tv_usec = 0;
47
48 rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
49 if (rc) {
50 PLOGE("setsockopt(SO_RCVTIMEO)");
51 goto err;
52 }
53
54 if (rport != 0) {
55 struct sockaddr_qrtr sq;
56
57 sq.sq_family = AF_QIPCRTR;
58 sq.sq_node = 1;
59 sq.sq_port = rport;
60
61 rc = bind(sock, (void *)&sq, sizeof(sq));
62 if (rc < 0) {
63 PLOGE("bind(%d)", rport);
64 goto err;
65 }
66 }
67
68 return sock;
69 err:
70 close(sock);
71 return -1;
72 }
73
qrtr_close(int sock)74 void qrtr_close(int sock)
75 {
76 close(sock);
77 }
78
qrtr_sendto(int sock,uint32_t node,uint32_t port,const void * data,unsigned int sz)79 int qrtr_sendto(int sock, uint32_t node, uint32_t port, const void *data, unsigned int sz)
80 {
81 struct sockaddr_qrtr sq;
82 int rc;
83
84 sq.sq_family = AF_QIPCRTR;
85 sq.sq_node = node;
86 sq.sq_port = port;
87
88 rc = sendto(sock, data, sz, 0, (void *)&sq, sizeof(sq));
89 if (rc < 0) {
90 PLOGE("sendto()");
91 return -1;
92 }
93
94 return 0;
95 }
96
qrtr_new_server(int sock,uint32_t service,uint16_t version,uint16_t instance)97 int qrtr_new_server(int sock, uint32_t service, uint16_t version, uint16_t instance)
98 {
99 struct qrtr_ctrl_pkt pkt;
100 struct sockaddr_qrtr sq;
101
102 if (qrtr_getname(sock, &sq))
103 return -1;
104
105 memset(&pkt, 0, sizeof(pkt));
106
107 pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
108 pkt.server.service = cpu_to_le32(service);
109 pkt.server.instance = cpu_to_le32(instance << 8 | version);
110
111 return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
112 }
113
qrtr_remove_server(int sock,uint32_t service,uint16_t version,uint16_t instance)114 int qrtr_remove_server(int sock, uint32_t service, uint16_t version, uint16_t instance)
115 {
116 struct qrtr_ctrl_pkt pkt;
117 struct sockaddr_qrtr sq;
118
119 if (qrtr_getname(sock, &sq))
120 return -1;
121
122 memset(&pkt, 0, sizeof(pkt));
123
124 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
125 pkt.server.service = cpu_to_le32(service);
126 pkt.server.instance = cpu_to_le32(instance << 8 | version);
127 pkt.server.node = cpu_to_le32(sq.sq_node);
128 pkt.server.port = cpu_to_le32(sq.sq_port);
129
130 return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
131 }
132
qrtr_publish(int sock,uint32_t service,uint16_t version,uint16_t instance)133 int qrtr_publish(int sock, uint32_t service, uint16_t version, uint16_t instance)
134 {
135 return qrtr_new_server(sock, service, version, instance);
136 }
137
qrtr_bye(int sock,uint32_t service,uint16_t version,uint16_t instance)138 int qrtr_bye(int sock, uint32_t service, uint16_t version, uint16_t instance)
139 {
140 return qrtr_remove_server(sock, service, version, instance);
141 }
142
qrtr_new_lookup(int sock,uint32_t service,uint16_t version,uint16_t instance)143 int qrtr_new_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance)
144 {
145 struct qrtr_ctrl_pkt pkt;
146 struct sockaddr_qrtr sq;
147
148 if (qrtr_getname(sock, &sq))
149 return -1;
150
151 memset(&pkt, 0, sizeof(pkt));
152
153 pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP);
154 pkt.server.service = cpu_to_le32(service);
155 pkt.server.instance = cpu_to_le32(instance << 8 | version);
156
157 return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
158 }
159
qrtr_remove_lookup(int sock,uint32_t service,uint16_t version,uint16_t instance)160 int qrtr_remove_lookup(int sock, uint32_t service, uint16_t version, uint16_t instance)
161 {
162 struct qrtr_ctrl_pkt pkt;
163 struct sockaddr_qrtr sq;
164
165 if (qrtr_getname(sock, &sq))
166 return -1;
167
168 memset(&pkt, 0, sizeof(pkt));
169
170 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_LOOKUP);
171 pkt.server.service = cpu_to_le32(service);
172 pkt.server.instance = cpu_to_le32(instance << 8 | version);
173 pkt.server.node = cpu_to_le32(sq.sq_node);
174 pkt.server.port = cpu_to_le32(sq.sq_port);
175
176 return qrtr_sendto(sock, sq.sq_node, QRTR_PORT_CTRL, &pkt, sizeof(pkt));
177 }
178
qrtr_poll(int sock,unsigned int ms)179 int qrtr_poll(int sock, unsigned int ms)
180 {
181 struct pollfd fds;
182
183 fds.fd = sock;
184 fds.revents = 0;
185 fds.events = POLLIN | POLLERR;
186
187 return poll(&fds, 1, ms);
188 }
189
qrtr_recv(int sock,void * buf,unsigned int bsz)190 int qrtr_recv(int sock, void *buf, unsigned int bsz)
191 {
192 int rc;
193
194 rc = recv(sock, buf, bsz, 0);
195 if (rc < 0)
196 PLOGE("recv()");
197 return rc;
198 }
199
qrtr_recvfrom(int sock,void * buf,unsigned int bsz,uint32_t * node,uint32_t * port)200 int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, uint32_t *node, uint32_t *port)
201 {
202 struct sockaddr_qrtr sq;
203 socklen_t sl;
204 int rc;
205
206 sl = sizeof(sq);
207 rc = recvfrom(sock, buf, bsz, 0, (void *)&sq, &sl);
208 if (rc < 0) {
209 PLOGE("recvfrom()");
210 return rc;
211 }
212 if (node)
213 *node = sq.sq_node;
214 if (port)
215 *port = sq.sq_port;
216 return rc;
217 }
218
qrtr_decode(struct qrtr_packet * dest,void * buf,size_t len,const struct sockaddr_qrtr * sq)219 int qrtr_decode(struct qrtr_packet *dest, void *buf, size_t len,
220 const struct sockaddr_qrtr *sq)
221 {
222 const struct qrtr_ctrl_pkt *ctrl = buf;
223
224 if (sq->sq_port == QRTR_PORT_CTRL){
225 if (len < sizeof(*ctrl))
226 return -EMSGSIZE;
227
228 dest->type = le32_to_cpu(ctrl->cmd);
229 switch (dest->type) {
230 case QRTR_TYPE_BYE:
231 dest->node = le32_to_cpu(ctrl->client.node);
232 break;
233 case QRTR_TYPE_DEL_CLIENT:
234 dest->node = le32_to_cpu(ctrl->client.node);
235 dest->port = le32_to_cpu(ctrl->client.port);
236 break;
237 case QRTR_TYPE_NEW_SERVER:
238 case QRTR_TYPE_DEL_SERVER:
239 dest->node = le32_to_cpu(ctrl->server.node);
240 dest->port = le32_to_cpu(ctrl->server.port);
241 dest->service = le32_to_cpu(ctrl->server.service);
242 dest->version = le32_to_cpu(ctrl->server.instance) & 0xff;
243 dest->instance = le32_to_cpu(ctrl->server.instance) >> 8;
244 break;
245 default:
246 dest->type = 0;
247 }
248 } else {
249 dest->type = QRTR_TYPE_DATA;
250 dest->node = sq->sq_node;
251 dest->port = sq->sq_port;
252
253 dest->data = buf;
254 dest->data_len = len;
255 }
256
257 return 0;
258 }
259