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  */
  32
  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>
  41
  42 #include "rpmsg.h"
  43
  44 #define REMOTE_RPROC_ID 4
  45 #define REMOTE_VPROC_ID 2
  46
  47 #define RECEIVE_PORT_ADDR 0
  48
  49 #define MESSAGEQ_RPMSG_MAXSIZE    512
  50 #define MESSAGEQ_RPMSG_PORT       61
  51
  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
  62
  63
  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;
  82
  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;
  88
  89 /*!
  90  *  @brief  Typedef for ease of use
  91  */
  92 typedef MessageQ_MsgHeader *MessageQ_Msg;
  93
  94 /*!
  95  *  @brief ti_rpmsg_send: Sends message through socket
  96  */
  97
  98 int ti_rpmsg_send(int sock, void *msg, int size)
  99 {
 100         int err;
 101
 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;
 109 }
 110
 111 /*!
 112  *  @brief ti_rpmsg_recv: receive message through socket
 113  */
 114
 115 int ti_rpmsg_recv(int sock, void *msg, int *byteCount)
 116 {
 117         int status = ECH0_TEST_SUCCESS;
 118         struct sockaddr_rpmsg fromAddr;  /* [Socket address of sender] */
 119         unsigned int  len;
 120
 121         memset(&fromAddr, 0, sizeof (fromAddr));
 122         len = sizeof (fromAddr);
 123
 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         }
 135
 136 exit:
 137         return status;
 138 }
 139
 140 /*!
 141  *  @brief ConnectSocket: Connect TX socket to destination address
 142  */
 143
 144 int ConnectSocket(int sock, uint16_t procId, int dst)
 145 {
 146         int                   err;
 147         struct sockaddr_rpmsg srcAddr, dstAddr;
 148         socklen_t             len;
 149         int vproc_id;
 150
 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;
 164
 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         }
 172
 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;
 185 }
 186
 187 /*!
 188  *  @brief SocketBindAddr: Bind RX socket to local address
 189  */
 190
 191 int SocketBindAddr(int fd, uint16_t procId, uint32_t localAddr)
 192 {
 193         int         err;
 194         socklen_t    len;
 195         struct sockaddr_rpmsg srcAddr;
 196         int vproc_id;
 197
 198
 199         /* map procId to vproc_id */
 200         if(procId == REMOTE_RPROC_ID) {
 201                 vproc_id = REMOTE_VPROC_ID;
 202         } else {
 203                 return -1;
 204         }
 205
 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;
 212
 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);
 219
 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);
 232 }
 233
 234 /*!
 235  *  @brief main: Main test code
 236  */
 237
 238 void main(void)
 239 {
 240
 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;
 251
 252         printf("Start of test");
 253         /* Create socket for sending messages to remote processor */
 254         sock = socket(AF_RPMSG, SOCK_SEQPACKET, 0);
 255
 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");
 264
 265         /* Connect socket to Msq RPMSG port */
 266         status = ConnectSocket(sock, REMOTE_RPROC_ID, MESSAGEQ_RPMSG_PORT);
 267
 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");
 276
 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         }
 282
 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");
 292
 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         }
 302
 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         }
 308
 309         printf("Socket Bind success !!\n");
 310
 311         memset(&init_msg,  0, sizeof(init_msg));
 312         msgSize = sizeof(init_msg);
 313
 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         }
 325
 326         /* Delay a bit for the init messages to be consumed */
 327         sleep(1);
 328
 329         msg = malloc(MESSAGEQ_RPMSG_MAXSIZE * sizeof(char));
 330         if (!msg) {
 331                 status = ECHO_TEST_ERR_MEMORY;
 332                 goto done ;
 333         }
 334
 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));
 347
 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                 }
 353
 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         }
 373
 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);
 390
 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);
 409
 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);
 419 }