1 /*
2 * Copyright (C) 2022 Intel Corporation.
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6 #include <errno.h>
7 #include <fcntl.h>
8 #include <stdio.h>
9 #include <stdlib.h>
10 #include <stdbool.h>
11 #include <string.h>
12 #include <termios.h>
13 #include <unistd.h>
14 #include <sys/types.h>
15 #include <sys/queue.h>
16 #include <sys/socket.h>
17 #include <netinet/in.h>
18 #include <arpa/inet.h>
19 #include "socket.h"
20 #include "log.h"
21 #include "mevent.h"
22
setup_and_listen_unix_socket(const char * sock_path,int num)23 static int setup_and_listen_unix_socket(const char *sock_path, int num)
24 {
25 struct sockaddr_un s_un;
26 int sock_fd, ret;
27
28 sock_fd = socket(AF_UNIX, SOCK_STREAM, 0);
29 if (sock_fd < 0)
30 goto err;
31 s_un.sun_family = AF_UNIX;
32 ret = snprintf(s_un.sun_path, sizeof(s_un.sun_path), "%s",
33 sock_path);
34 if (ret < 0 || ret >= sizeof(s_un.sun_path))
35 goto err_close_socket;
36
37 if (bind(sock_fd, (struct sockaddr *)&s_un, sizeof(s_un)) < 0)
38 goto err_close_socket;
39 if (listen(sock_fd, num) < 0)
40 goto err_close_socket;
41 pr_info("Listening on: %s, socket fd = %d.\r\n", sock_path, sock_fd);
42 return sock_fd;
43 err_close_socket:
44 close(sock_fd);
45 err:
46 return -1;
47 }
free_socket_client(struct socket_dev * sock,struct socket_client * client)48 static void free_socket_client(struct socket_dev *sock, struct socket_client *client)
49 {
50 pthread_mutex_t *per_client_mutex = client->per_client_mutex;
51 pthread_mutex_lock(&sock->client_mtx);
52 LIST_REMOVE(client, list);
53 pthread_mutex_unlock(&sock->client_mtx);
54
55 if (per_client_mutex) {
56 pthread_mutex_lock(per_client_mutex);
57 }
58 if (client->free_client_cb) {
59 client->free_client_cb(client);
60 }
61 close(client->fd);
62 client->fd = -1;
63 free(client);
64 if (per_client_mutex) {
65 pthread_mutex_unlock(per_client_mutex);
66 }
67 }
68
write_socket_char(struct socket_client * client)69 int write_socket_char(struct socket_client *client)
70 {
71 struct msghdr msg;
72 struct iovec iov[1];
73 int ret;
74 char control[CMSG_SPACE(sizeof(int))];
75 struct cmsghdr *cmsg;
76
77 memset(&msg, 0, sizeof(msg));
78 memset(control, 0, sizeof(control));
79
80 iov[0].iov_base = (void *)client->buf;
81 iov[0].iov_len = client->len;
82
83 msg.msg_iov = iov;
84 msg.msg_iovlen = 1;
85
86 msg.msg_control = control;
87 msg.msg_controllen = sizeof(control);
88
89 cmsg = CMSG_FIRSTHDR(&msg);
90 cmsg->cmsg_len = CMSG_LEN(sizeof(int));
91 cmsg->cmsg_level = SOL_SOCKET;
92 cmsg->cmsg_type = SCM_RIGHTS;
93 memcpy(CMSG_DATA(cmsg), &client->fd, sizeof(int));
94
95 do {
96 ret = sendmsg(client->fd, &msg, 0);
97 } while (ret < 0 && errno == EINTR);
98
99 return ret;
100 }
101
read_socket_char(struct socket_dev * sock,struct socket_client * client)102 int read_socket_char(struct socket_dev *sock, struct socket_client *client)
103 {
104 struct iovec iov;
105 struct msghdr msg;
106 struct cmsghdr *cmsg;
107 char buf[CMSG_SPACE(sizeof(int))];
108 int fdflags_recvmsg = MSG_CMSG_CLOEXEC;
109
110 memset(&msg, 0, sizeof(msg));
111 memset(client->buf, '\0', CLIENT_BUF_LEN);
112 iov.iov_base = client->buf;
113 iov.iov_len = CLIENT_BUF_LEN;
114 msg.msg_iov = &iov;
115 msg.msg_iovlen = 1;
116 msg.msg_name = NULL;
117 msg.msg_namelen = 0;
118
119 msg.msg_control = buf;
120 msg.msg_controllen = sizeof(buf);
121 cmsg = CMSG_FIRSTHDR(&msg);
122 cmsg->cmsg_level = SOL_SOCKET;
123 cmsg->cmsg_type = SCM_RIGHTS;
124 cmsg->cmsg_len = CMSG_LEN(sizeof(int));
125
126 client->len = recvmsg(client->fd, &msg, fdflags_recvmsg);
127 if (client->len <= 0) {
128 pr_err("Socket Disconnect: %d.\r\n", client->fd);
129 free_socket_client(sock, client);
130 sock->num_client--;
131 return -1;
132 }
133 if (client->len == CLIENT_BUF_LEN) {
134 pr_err("Socket buffer overflow!\r\n");
135 return -1;
136 }
137 for (unsigned int i = 0U; i < CLIENT_BUF_LEN; i++)
138 if (client->buf[i] == 0xa)
139 client->buf[i] = '\0';
140 pr_info("Receive data:(%s)\n", client->buf);
141 if (sock->data_handler != NULL)
142 sock->data_handler(client->buf, client->fd);
143 return 0;
144 }
new_socket_client(struct socket_dev * sock)145 static struct socket_client *new_socket_client(struct socket_dev *sock)
146 {
147 struct socket_client *client;
148
149 client = calloc(1, sizeof(*client));
150 if (!client) {
151 pr_err("%s: failed to allocate memory for client\n",
152 __func__);
153 goto alloc_client;
154 }
155 /* If per client mutex is needed, init in callback */
156 client->per_client_mutex = NULL;
157 client->addr_len = sizeof(client->addr);
158 client->fd =
159 accept(sock->sock_fd, (struct sockaddr *)&client->addr,
160 &client->addr_len);
161 if (client->fd < 0) {
162 if (sock->listening)
163 pr_err("%s: Failed to accept from fd %d, err: %s\n",
164 __func__, sock->sock_fd, strerror(errno));
165 goto accept_con;
166 }
167 pthread_mutex_lock(&sock->client_mtx);
168 LIST_INSERT_HEAD(&sock->client_head, client, list);
169 pthread_mutex_unlock(&sock->client_mtx);
170
171 return client;
172
173 accept_con:
174 free(client);
175 alloc_client:
176 return NULL;
177 }
listen_socket_client(void * arg)178 static void *listen_socket_client(void *arg)
179 {
180 struct socket_dev *sock = (struct socket_dev *)arg;
181 struct socket_client *client;
182
183 pr_info("Socket Listening %d...\n", sock->sock_fd);
184 while (sock->listening) {
185 /* wait connection */
186 if (sock->num_client >= SOCKET_MAX_CLIENT) {
187 usleep(500000);
188 continue;
189 }
190
191 client = new_socket_client(sock);
192 if (!client) {
193 usleep(500000);
194 continue;
195 }
196 pr_info("Socket Connected:%d\n", client->fd);
197 sock->num_client++;
198 }
199 pr_info("Stop listening %d.\n", sock->sock_fd);
200 return NULL;
201 }
socket_poll_events(void * arg)202 static void *socket_poll_events(void *arg)
203 {
204 struct socket_dev *sock = (struct socket_dev *)arg;
205 struct socket_client *client;
206 fd_set rfd;
207 int max_fd = 0;
208 struct timeval timeout;
209 struct socket_client *poll_client[SOCKET_MAX_CLIENT];
210 int nfd, i;
211
212 pr_info("Polling socket %d.\n", sock->sock_fd);
213 while (sock->polling) {
214 max_fd = 0;
215 nfd = 0;
216 pthread_mutex_lock(&sock->client_mtx);
217 FD_ZERO(&rfd);
218 LIST_FOREACH(client, &sock->client_head, list) {
219 FD_SET(client->fd, &rfd);
220 poll_client[nfd] = client;
221 nfd++;
222 if (client->fd > max_fd)
223 max_fd = client->fd;
224 }
225 pthread_mutex_unlock(&sock->client_mtx);
226
227 timeout.tv_sec = 0;
228 timeout.tv_usec = 10000;
229 select(max_fd + 1, &rfd, NULL, NULL, &timeout);
230
231 for (i = 0; i < nfd; i++) {
232 client = poll_client[i];
233 if (!FD_ISSET(client->fd, &rfd))
234 continue;
235
236 if (read_socket_char(sock, client) < 0)
237 continue;
238 }
239 }
240 pr_info("Stop polling on socket %d.\n", sock->sock_fd);
241
242 return NULL;
243 }
find_socket_client(struct socket_dev * sock,int fd)244 struct socket_client *find_socket_client(struct socket_dev *sock, int fd)
245 {
246 struct socket_client *client = NULL;
247 pthread_mutex_lock(&sock->client_mtx);
248 LIST_FOREACH(client, &sock->client_head, list) {
249 if (client->fd == fd)
250 break;
251 }
252 pthread_mutex_unlock(&sock->client_mtx);
253 return client;
254 }
open_socket(struct socket_dev * sock,data_handler_f * fn)255 int open_socket(struct socket_dev *sock, data_handler_f *fn)
256 {
257 sock->listening = true;
258 sock->polling = true;
259 sock->data_handler = fn;
260 pthread_mutex_init(&sock->client_mtx, NULL);
261 unlink(sock->unix_sock_path);
262 sock->sock_fd = setup_and_listen_unix_socket(sock->unix_sock_path, SOCKET_MAX_CLIENT);
263 if (sock->sock_fd < 0)
264 return -1;
265 pthread_create(&sock->listen_thread, NULL, listen_socket_client, sock);
266 pthread_create(&sock->connect_thread, NULL, socket_poll_events, sock);
267 return 0;
268 }
269
close_socket(struct socket_dev * sock)270 void close_socket(struct socket_dev *sock)
271 {
272 struct socket_client *client, *tclient;
273
274 sock->listening = false;
275 sock->polling = false;
276 shutdown(sock->sock_fd, SHUT_RDWR);
277
278 pthread_join(sock->listen_thread, NULL);
279 pthread_join(sock->connect_thread, NULL);
280
281 pthread_mutex_lock(&sock->client_mtx);
282 list_foreach_safe(client, &sock->client_head, list, tclient) {
283 LIST_REMOVE(client, list);
284 close(client->fd);
285 client->fd = -1;
286 free(client);
287 }
288 pthread_mutex_unlock(&sock->client_mtx);
289
290 close(sock->sock_fd);
291 unlink(sock->unix_sock_path);
292 }
init_socket(char * path)293 struct socket_dev *init_socket(char *path)
294 {
295 struct socket_dev *sock;
296
297 sock = calloc(1, sizeof(*sock));
298 if (!sock) {
299 pr_err("%s: Failed to allocate memory for socket\n", __func__);
300 return NULL;
301 }
302 memset(sock, 0x0, sizeof(struct socket_dev));
303 strncpy(sock->unix_sock_path, path, UNIX_SOCKET_PATH_MAX - 1);
304 return sock;
305 }
deinit_socket(struct socket_dev * sock)306 void deinit_socket(struct socket_dev *sock)
307 {
308 if (sock != NULL)
309 free(sock);
310 }