1 /*
2 * Copyright (C)2021-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 "list.h"
22
23
setup_and_listen_unix_socket(const char * sock_path,int num)24 static int setup_and_listen_unix_socket(const char *sock_path, int num)
25 {
26 struct sockaddr_un s_un;
27 int sock_fd, ret;
28
29 sock_fd = socket(AF_UNIX, SOCK_STREAM, 0);
30 if (sock_fd < 0)
31 goto err;
32 s_un.sun_family = AF_UNIX;
33 ret = snprintf(s_un.sun_path, sizeof(s_un.sun_path), "%s",
34 sock_path);
35 if (ret < 0 || ret >= sizeof(s_un.sun_path))
36 goto err_close_socket;
37
38 if (bind(sock_fd, (struct sockaddr *)&s_un, sizeof(s_un)) < 0)
39 goto err_close_socket;
40 if (listen(sock_fd, num) < 0)
41 goto err_close_socket;
42 LOG_PRINTF("Start to listen:%s\r\n", sock_path);
43 return sock_fd;
44 err_close_socket:
45 close(sock_fd);
46 err:
47 return -1;
48 }
free_socket_client(struct socket_dev * sock,struct socket_client * client)49 static void free_socket_client(struct socket_dev *sock, struct socket_client *client)
50 {
51 pthread_mutex_lock(&sock->client_mtx);
52 LIST_REMOVE(client, list);
53 pthread_mutex_unlock(&sock->client_mtx);
54
55 close(client->fd);
56 client->fd = -1;
57 free(client);
58 }
59
write_socket_char(struct socket_client * client)60 int write_socket_char(struct socket_client *client)
61 {
62 struct msghdr msg;
63 struct iovec iov[1];
64 int ret;
65 char control[CMSG_SPACE(sizeof(int))];
66 struct cmsghdr *cmsg;
67
68 memset(&msg, 0, sizeof(msg));
69 memset(control, 0, sizeof(control));
70
71 iov[0].iov_base = (void *)client->buf;
72 iov[0].iov_len = client->len;
73
74 msg.msg_iov = iov;
75 msg.msg_iovlen = 1;
76
77 msg.msg_control = control;
78 msg.msg_controllen = sizeof(control);
79
80 cmsg = CMSG_FIRSTHDR(&msg);
81 cmsg->cmsg_len = CMSG_LEN(sizeof(int));
82 cmsg->cmsg_level = SOL_SOCKET;
83 cmsg->cmsg_type = SCM_RIGHTS;
84 memcpy(CMSG_DATA(cmsg), &client->fd, sizeof(int));
85
86 do {
87 ret = sendmsg(client->fd, &msg, 0);
88 } while (ret < 0 && errno == EINTR);
89
90 return ret;
91 }
parse_socket_client_name(struct socket_client * client)92 static void parse_socket_client_name(struct socket_client *client)
93 {
94 char *saveptr;
95
96 (void) strtok_r(client->buf, ":", &saveptr);
97 if (strlen(saveptr) > 0) {
98 strncpy(client->name, saveptr, SOCKET_CLIENT_NAME_MAX - 1U);
99 LOG_PRINTF("Socket client name:%s\n", client->name);
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 LOG_PRINTF("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 LOG_WRITE("Socket buf 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 LOG_PRINTF("Receive data:(%s)\n", client->buf);
141 parse_socket_client_name(client);
142 if (sock->data_handler != NULL)
143 sock->data_handler(client->buf, client->fd);
144 return 0;
145 }
new_socket_client(struct socket_dev * sock)146 static struct socket_client *new_socket_client(struct socket_dev *sock)
147 {
148 struct socket_client *client;
149
150 client = calloc(1, sizeof(*client));
151 if (!client) {
152 LOG_PRINTF("%s: failed to allocate memory for client\n",
153 __func__);
154 goto alloc_client;
155 }
156
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 LOG_PRINTF("%s: Failed to accept from fd %d, err: %s\n",
164 __func__, sock->sock_fd, strerror(errno));
165 goto accept_con;
166 }
167
168 pthread_mutex_lock(&sock->client_mtx);
169 LIST_INSERT_HEAD(&sock->client_head, client, list);
170 pthread_mutex_unlock(&sock->client_mtx);
171
172 return client;
173
174 accept_con:
175 free(client);
176 alloc_client:
177 return NULL;
178 }
listen_socket_client(void * arg)179 static void *listen_socket_client(void *arg)
180 {
181 struct socket_dev *sock = (struct socket_dev *)arg;
182 struct socket_client *client;
183
184 LOG_PRINTF("Socket Listening %d...\n", sock->sock_fd);
185 while (sock->listening) {
186 /* wait connection */
187 if (sock->num_client >= SOCKET_MAX_CLIENT) {
188 usleep(500000);
189 continue;
190 }
191
192 client = new_socket_client(sock);
193 if (!client) {
194 usleep(500000);
195 continue;
196 }
197 LOG_PRINTF("Socket Connected:%d\n", client->fd);
198 sock->num_client++;
199 }
200 LOG_PRINTF("Stop listening %d...\n", sock->sock_fd);
201 return NULL;
202 }
socket_poll_events(void * arg)203 static void *socket_poll_events(void *arg)
204 {
205 struct socket_dev *sock = (struct socket_dev *)arg;
206 struct socket_client *client;
207 fd_set rfd;
208 int max_fd = 0;
209 struct timeval timeout;
210 struct socket_client *poll_client[SOCKET_MAX_CLIENT];
211 int nfd, i;
212
213 LOG_PRINTF("Socket polling %d...\n", sock->sock_fd);
214 while (sock->polling) {
215 max_fd = 0;
216 nfd = 0;
217 pthread_mutex_lock(&sock->client_mtx);
218 FD_ZERO(&rfd);
219 LIST_FOREACH(client, &sock->client_head, list) {
220 FD_SET(client->fd, &rfd);
221 poll_client[nfd] = client;
222 nfd++;
223 if (client->fd > max_fd)
224 max_fd = client->fd;
225 }
226 pthread_mutex_unlock(&sock->client_mtx);
227
228 timeout.tv_sec = 0;
229 timeout.tv_usec = 10000;
230 select(max_fd + 1, &rfd, NULL, NULL, &timeout);
231
232 for (i = 0; i < nfd; i++) {
233 client = poll_client[i];
234 if (!FD_ISSET(client->fd, &rfd))
235 continue;
236
237 if (read_socket_char(sock, client) < 0)
238 continue;
239 }
240 }
241 LOG_PRINTF("Socket Stop polling %d...\n", sock->sock_fd);
242
243 return NULL;
244 }
find_socket_client(struct socket_dev * sock,int fd)245 struct socket_client *find_socket_client(struct socket_dev *sock, int fd)
246 {
247 struct socket_client *client = NULL;
248 pthread_mutex_lock(&sock->client_mtx);
249 LIST_FOREACH(client, &sock->client_head, list) {
250 if (client->fd == fd)
251 break;
252 }
253 pthread_mutex_unlock(&sock->client_mtx);
254 return client;
255 }
open_socket(struct socket_dev * sock,data_handler_f * fn)256 int open_socket(struct socket_dev *sock, data_handler_f *fn)
257 {
258 sock->listening = true;
259 sock->polling = true;
260 sock->data_handler = fn;
261 pthread_mutex_init(&sock->client_mtx, NULL);
262 unlink(sock->unix_sock_path);
263 sock->sock_fd = setup_and_listen_unix_socket(sock->unix_sock_path, SOCKET_MAX_CLIENT);
264 if (sock->sock_fd < 0)
265 return -1;
266 pthread_create(&sock->listen_thread, NULL, listen_socket_client, sock);
267 pthread_create(&sock->connect_thread, NULL, socket_poll_events, sock);
268 return 0;
269 }
270
close_socket(struct socket_dev * sock)271 void close_socket(struct socket_dev *sock)
272 {
273 struct socket_client *client, *tclient;
274
275 sock->listening = false;
276 sock->polling = false;
277 shutdown(sock->sock_fd, SHUT_RDWR);
278
279 pthread_join(sock->listen_thread, NULL);
280 pthread_join(sock->connect_thread, NULL);
281
282 pthread_mutex_lock(&sock->client_mtx);
283 list_foreach_safe(client, &sock->client_head, list, tclient) {
284 LIST_REMOVE(client, list);
285 close(client->fd);
286 client->fd = -1;
287 free(client);
288 }
289 pthread_mutex_unlock(&sock->client_mtx);
290
291 close(sock->sock_fd);
292 unlink(sock->unix_sock_path);
293 }
init_socket(char * path)294 struct socket_dev *init_socket(char *path)
295 {
296 struct socket_dev *sock;
297
298 sock = calloc(1, sizeof(*sock));
299 if (!sock) {
300 LOG_PRINTF("%s: Failed to allocate memory for socket\n", __func__);
301 return NULL;
302 }
303 memset(sock, 0x0, sizeof(struct socket_dev));
304 strncpy(sock->unix_sock_path, path, UNIX_SOCKET_PATH_MAX - 1);
305 return sock;
306 }
deinit_socket(struct socket_dev * sock)307 void deinit_socket(struct socket_dev *sock)
308 {
309 if (sock != NULL)
310 free(sock);
311 }