test: Add rpmsg proto socket test code
[processor-sdk/ti-openamp-test.git] / rpmsg_proto_socket_test.c
1 /*
2  * Copyright (c) 2017, Texas Instruments Incorporated
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * *  Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  *
12  * *  Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  *
16  * *  Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
33 #include <stdio.h>
34 #include <stdint.h>
35 #include <stdlib.h>
36 #include <string.h>
37 #include <fcntl.h>
38 #include <unistd.h>
39 #include <sys/socket.h>
40 #include <errno.h>
42 #include "rpmsg.h"
44 #define REMOTE_RPROC_ID 4
45 #define REMOTE_VPROC_ID 2
47 #define RECEIVE_PORT_ADDR 0
49 #define MESSAGEQ_RPMSG_MAXSIZE    512
50 #define MESSAGEQ_RPMSG_PORT       61
52 #define ECH0_TEST_SUCCESS            0
53 /* Error messages */
54 #define ECHO_TEST_TX_SOCK_CREATE_FAIL  -1
55 #define ECHO_TEST_TX_SOCK_CONNECT_FAIL -2
56 #define ECHO_TEST_RX_SOCK_CREATE_FAIL  -3
57 #define ECHO_TEST_RX_SOCK_BIND_FAIL    -4
58 #define ECHO_TEST_ERR_MEMORY           -5
59 #define ECHO_TEST_ERR_RECV_FAIL        -6
60 #define ECHO_TEST_ERR_RECV_SIZE_FAIL   -7
61 #define ECHO_TEST_MESSAGE_MISMATCH     -8
64 /*!
65  *  @brief      Required first field in every message
66  */
67 typedef struct {
68         uint32_t       reserved0;         /*!< reserved for List.elem->next       */
69         uint32_t       reserved1;         /*!< reserved for List.elem->prev       */
70         uint32_t       msgSize;           /*!< message size                       */
71         uint16_t       flags;             /*!< bitmask of different flags         */
72         uint16_t       msgId;             /*!< message id                         */
73         uint16_t       dstId;             /*!< destination queue id               */
74         uint16_t       dstProc;           /*!< destination processor id           */
75         uint16_t       replyId;           /*!< reply id                           */
76         uint16_t       replyProc;         /*!< reply processor                    */
77         uint16_t       srcProc;           /*!< source processor                   */
78         uint16_t       heapId;            /*!< heap id                            */
79         uint16_t       seqNum;            /*!< sequence number                    */
80         uint16_t       reserved;          /*!< reserved                           */
81 } MessageQ_MsgHeader;
83 typedef struct {
84         uint32_t       reserved0;         /*!< reserved for List.elem->next       */
85         uint32_t       reserved1;         /*!< reserved for List.elem->prev       */
86         uint32_t       dstChannelAddr;      /*!< message size                       */
87 } Init_MsgHeader;
89 /*!
90  *  @brief  Typedef for ease of use
91  */
92 typedef MessageQ_MsgHeader *MessageQ_Msg;
94 /*!
95  *  @brief ti_rpmsg_send: Sends message through socket
96  */
98 int ti_rpmsg_send(int sock, void *msg, int size)
99 {
100         int err;
102         err = send(sock, msg, size, 0);
103         if (err < 0) {
104                 fprintf(stderr, "ti_rpmsg_send: send failed: %d (%s)\n",
105                         errno, strerror(errno));
106                 return -1;
107         }
108         return 0;
111 /*!
112  *  @brief ti_rpmsg_recv: receive message through socket
113  */
115 int ti_rpmsg_recv(int sock, void *msg, int *byteCount)
117         int status = ECH0_TEST_SUCCESS;
118         struct sockaddr_rpmsg fromAddr;  /* [Socket address of sender] */
119         unsigned int  len;
121         memset(&fromAddr, 0, sizeof (fromAddr));
122         len = sizeof (fromAddr);
124         *byteCount = recvfrom(sock, msg, MESSAGEQ_RPMSG_MAXSIZE, 0,
125                               (struct sockaddr *)&fromAddr, &len);
126         if (len != sizeof (fromAddr)) {
127                 fprintf(stderr, "recvfrom: got bad addr len (%d)\n", len);
128                 status = ECHO_TEST_ERR_RECV_FAIL;
129                 goto exit;
130         }
131         if (*byteCount < 0) {
132                 fprintf(stderr, "recvfrom: got bad addr len (%d)\n", len);
133                 status = ECHO_TEST_ERR_RECV_SIZE_FAIL;          
134         }
136 exit:
137         return status;
140 /*!
141  *  @brief ConnectSocket: Connect TX socket to destination address
142  */
144 int ConnectSocket(int sock, uint16_t procId, int dst)
146         int                   err;
147         struct sockaddr_rpmsg srcAddr, dstAddr;
148         socklen_t             len;
149         int vproc_id;
151         /* TODO: Add table conversion for rpoc to vproc
152            Need to handle platform specific part properly */
153         if(procId == REMOTE_RPROC_ID) {
154                 vproc_id = REMOTE_VPROC_ID;
155         } else {
156                 return -1;
157         }
158         /* connect to remote service */
159         memset(&dstAddr, 0, sizeof(dstAddr));
160         dstAddr.family     = AF_RPMSG;
161         /* convert MultiProc 'clusterId' to remoteproc index */
162         dstAddr.vproc_id   = vproc_id;
163         dstAddr.addr       = dst;
165         len = sizeof(struct sockaddr_rpmsg);
166         err = connect(sock, (struct sockaddr *)&dstAddr, len);
167         if (err < 0) {
168                 /* don't hard-printf since this is no longer fatal */
169                 fprintf(stderr, "connect failed: %s (%d)\n", strerror(errno), errno);
170                 return (-1);
171         }
173         /* let's see what local address we got */
174         err = getsockname(sock, (struct sockaddr *)&srcAddr, &len);
175         if (err < 0) {
176                 fprintf(stderr, "getpeername failed: %s (%d)\n",
177                         strerror(errno), errno);
178                 return (-1);
179         }
180         printf("Connected over sock: %d\n\tdst vproc_id: %d, dst addr: %d\n",
181                sock, dstAddr.vproc_id, dstAddr.addr);
182         printf("\tsrc vproc_id: %d, src addr: %d\n",
183                srcAddr.vproc_id, srcAddr.addr);
184         return 0;
187 /*!
188  *  @brief SocketBindAddr: Bind RX socket to local address
189  */
191 int SocketBindAddr(int fd, uint16_t procId, uint32_t localAddr)
193         int         err;
194         socklen_t    len;
195         struct sockaddr_rpmsg srcAddr;
196         int vproc_id;
199         /* map procId to vproc_id */
200         if(procId == REMOTE_RPROC_ID) {
201                 vproc_id = REMOTE_VPROC_ID;
202         } else {
203                 return -1;
204         }
206         /* Now bind to the source address.   */
207         memset(&srcAddr, 0, sizeof(srcAddr));
208         srcAddr.family = AF_RPMSG;
209         /* We bind the remote clusterId, but local address! */
210         srcAddr.vproc_id   = vproc_id;
211         srcAddr.addr  = localAddr;
213         len = sizeof(struct sockaddr_rpmsg);
214         err = bind(fd, (struct sockaddr *)&srcAddr, len);
215         if (err >= 0) {
216                 printf("socket_bind_addr: bound sock: %d\n\tto dst "
217                        "vproc_id: %d, src addr: %d\n",
218                        fd, srcAddr.vproc_id, srcAddr.addr);
220                 /* let's see what local address we got */
221                 err = getsockname(fd, (struct sockaddr *)&srcAddr, &len);
222                 if (err < 0) {
223                         fprintf(stderr, "getsockname failed: %s (%d)\n",
224                                 strerror(errno), errno);
225                 }
226                 else {
227                         printf("\tsrc vproc_id: %d, src addr: %d\n",
228                                srcAddr.vproc_id, srcAddr.addr);
229                 }
230         }
231         return (err);
234 /*!
235  *  @brief main: Main test code
236  */
238 void main(void)
241         int sock=0;
242         int status;
243         int flags;
244         MessageQ_Msg msg=NULL, rcv_msg=NULL;
245         Init_MsgHeader init_msg;
246         int i, j;
247         int recv_sock=0;
248         struct sockaddr_rpmsg srcAddr;
249         int msgSize, rcvMsgSize;
250         uint32_t *dataPtr;
252         printf("Start of test");
253         /* Create socket for sending messages to remote processor */
254         sock = socket(AF_RPMSG, SOCK_SEQPACKET, 0);
256         if (sock < 0) {
257                 status = ECHO_TEST_TX_SOCK_CREATE_FAIL;
258                 fprintf(stderr,
259                         "main: socket failed: error %d (%s)\n", errno,
260                         strerror(errno));
261                 goto done;
262         }
263         printf("Tx Socket create success !!\n");
265         /* Connect socket to Msq RPMSG port */
266         status = ConnectSocket(sock, REMOTE_RPROC_ID, MESSAGEQ_RPMSG_PORT);
268         if (status < 0) {
269                 status = ECHO_TEST_TX_SOCK_CONNECT_FAIL;
270                 fprintf(stderr,
271                         "main: connect failed: error %d (%s) procId: %d\n",
272                         errno, strerror(errno), REMOTE_RPROC_ID);
273                 goto done;
274         }
275         printf("Connect Tx Socket success !!\n");
277         /* Make sure socket fd doesn't exist for 'fork() -> exec*()'ed child */
278         flags = fcntl(sock, F_GETFD);
279         if (flags != -1) {
280                 fcntl(sock, F_SETFD, flags | FD_CLOEXEC);
281         }
283         /*  Create the socket to receive messages */
284         recv_sock = socket(AF_RPMSG, SOCK_SEQPACKET, 0);
285         if (recv_sock < 0) {
286                 fprintf(stderr, "TransportRpmsg_bind: socket call failed: %d (%s)\n",
287                         errno, strerror(errno));
288                 status = ECHO_TEST_RX_SOCK_CREATE_FAIL;
289                 goto done;
290         }
291         printf("Rx Socket create success !!\n");
293         /* Bind socket to Recieve port address */
294         status = SocketBindAddr(recv_sock, REMOTE_RPROC_ID, RECEIVE_PORT_ADDR );
295         if (status < 0) {
296                 /* don't hard-printf since this is no longer fatal */
297                 fprintf(stderr, "TransportRpmsg_bind: bind failed: %d (%s)\n",
298                        errno, strerror(errno));
299                 status = ECHO_TEST_RX_SOCK_BIND_FAIL;
300                 goto done;
301         }
303         /* Make sure socket fd doesn't exist for 'fork() -> exec*()'ed child */
304         flags = fcntl(recv_sock, F_GETFD);
305         if (flags != -1) {
306                 fcntl(recv_sock, F_SETFD, flags | FD_CLOEXEC);
307         }
309         printf("Socket Bind success !!\n");
311         memset(&init_msg,  0, sizeof(init_msg));
312         msgSize = sizeof(init_msg);
314         for(i=0; i< 2; i++) {
315                 /* Send init message twice to prime*/
316                 status = ti_rpmsg_send(sock, &init_msg, msgSize);
317                 if (status < 0) {
318                         fprintf(stderr,
319                                 "main: ti_rpmsg_send %d failed %d (%s)\n", i, errno,
320                                 strerror(errno));
321                         goto done;
322                 }
323                 printf("rpmsg send init message:%d success; size %d!!\n", i, msgSize);
324         }
326         /* Delay a bit for the init messages to be consumed */
327         sleep(1);
329         msg = malloc(MESSAGEQ_RPMSG_MAXSIZE * sizeof(char));
330         if (!msg) {
331                 status = ECHO_TEST_ERR_MEMORY;
332                 goto done ;
333         }
335         msgSize = MESSAGEQ_RPMSG_MAXSIZE-sizeof(MessageQ_MsgHeader);
336         msg->msgSize = MESSAGEQ_RPMSG_MAXSIZE-sizeof(MessageQ_MsgHeader);
337         msg->flags = 0;
338         msg->msgId = 0;
339         msg->dstId = REMOTE_RPROC_ID;
340         msg->dstProc = 0;
341         msg->replyId = 1;
342         msg->replyProc = 1;
343         msg->srcProc = 0;
344         msg->heapId = 0;
345         msg->seqNum = 0;
346         dataPtr = (uint32_t *)((char *)(msg) +sizeof(MessageQ_MsgHeader));
348         for(i=0; i< 1; i++) {
349                 msg->seqNum = i;
350                 for(j=0; j<msgSize>>2; j++) {
351                         dataPtr[j] = j+msg->seqNum;
352                 }
354                 status = ti_rpmsg_send(sock, msg, msgSize);
355                 if (status < 0) {
356                         fprintf(stderr,
357                                 "main: ti_rpmsg_send %d failed %d (%s)\n", i, errno,
358                                 strerror(errno));
359                         goto done;
360                 }
361                 fprintf(stderr, "ti_rpmsg_send:%d success! Size %d!!\n", i, msgSize);
362         }
363         /*
364          * We have no way of peeking to see what message size we'll get, so we
365          * allocate a message of max size to receive contents from the rpmsg socket
366          * (currently, a copy transport)
367          */
368         rcv_msg = malloc(MESSAGEQ_RPMSG_MAXSIZE*sizeof(char));
369         if (!rcv_msg) {
370                 status = ECHO_TEST_ERR_MEMORY;
371                 goto done;
372         }
374         do {
375                 /* Receive message */
376                 status = ti_rpmsg_recv(recv_sock, rcv_msg, &rcvMsgSize);
377                 if (status < 0) {
378                         fprintf(stderr, "ti_rpmsg_recv failed! status %d, rcvMsgSize %d\n",
379                                 status, rcvMsgSize);
380                         goto done;
381                 }
382                 if (rcvMsgSize < 16) {
383                         /* Ignore any init messages returned */
384                         printf("ti_rpmsg_recv: receive dummy message success size %d!!\n",
385                                rcvMsgSize);
386                 } else {
387                         printf("ti_rpmsg_recv: receive success! Size %d!!\n", rcvMsgSize);
388                 }
389         } while (rcvMsgSize < 16);
391 #ifdef DEBUG
392         for (i=0; i<rcvMsgSize; i+=4) {
393                 printf("%x %x %x %x\n",
394                        *(((uint8_t *)(rcv_msg))+i),
395                        *(((uint8_t *)(rcv_msg))+i+1),
396                        *(((uint8_t *)(rcv_msg))+i+2),
397                        *(((uint8_t *)(rcv_msg))+i+3)
398                        );
399         }
400 #endif
401         printf("Check receive msg complete !!\n");
402         status = memcmp(rcv_msg, msg, rcvMsgSize);
403         if(status) {
404                 fprintf(stderr, "Message mismatch %d !!\n", status);
405                 status = ECHO_TEST_MESSAGE_MISMATCH;
406                 goto done;
407         }
408         printf("Test Pass: Messages match size %d, status %d !!\n", rcvMsgSize, status);
410 done:
411         if (recv_sock)
412                 close(recv_sock);
413         if (sock)
414                 close(sock);
415         if(rcv_msg)
416                 free(rcv_msg);
417         if(msg)
418                 free(msg);