/*
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/socket.h>
#include <errno.h>

#include "rpmsg.h"

#define REMOTE_RPROC_ID 4
#define REMOTE_VPROC_ID 2

#define RECEIVE_PORT_ADDR 0

#define MESSAGEQ_RPMSG_MAXSIZE    512
#define MESSAGEQ_RPMSG_PORT       61

#define ECH0_TEST_SUCCESS            0
/* Error messages */
#define ECHO_TEST_TX_SOCK_CREATE_FAIL  -1
#define ECHO_TEST_TX_SOCK_CONNECT_FAIL -2
#define ECHO_TEST_RX_SOCK_CREATE_FAIL  -3
#define ECHO_TEST_RX_SOCK_BIND_FAIL    -4
#define ECHO_TEST_ERR_MEMORY           -5
#define ECHO_TEST_ERR_RECV_FAIL        -6
#define ECHO_TEST_ERR_RECV_SIZE_FAIL   -7
#define ECHO_TEST_MESSAGE_MISMATCH     -8


/*!
 *  @brief      Required first field in every message
 */
typedef struct {
	uint32_t       reserved0;         /*!< reserved for List.elem->next       */
	uint32_t       reserved1;         /*!< reserved for List.elem->prev       */
	uint32_t       msgSize;           /*!< message size                       */
	uint16_t       flags;             /*!< bitmask of different flags         */
	uint16_t       msgId;             /*!< message id                         */
	uint16_t       dstId;             /*!< destination queue id               */
	uint16_t       dstProc;           /*!< destination processor id           */
	uint16_t       replyId;           /*!< reply id                           */
	uint16_t       replyProc;         /*!< reply processor                    */
	uint16_t       srcProc;           /*!< source processor                   */
	uint16_t       heapId;            /*!< heap id                            */
	uint16_t       seqNum;            /*!< sequence number                    */
	uint16_t       reserved;          /*!< reserved                           */
} MessageQ_MsgHeader;

typedef struct {
	uint32_t       reserved0;         /*!< reserved for List.elem->next       */
	uint32_t       reserved1;         /*!< reserved for List.elem->prev       */
	uint32_t       dstChannelAddr;      /*!< message size                       */
} Init_MsgHeader;

/*!
 *  @brief  Typedef for ease of use
 */
typedef MessageQ_MsgHeader *MessageQ_Msg;

/*!
 *  @brief ti_rpmsg_send: Sends message through socket
 */

int ti_rpmsg_send(int sock, void *msg, int size)
{
	int err;

	err = send(sock, msg, size, 0);
	if (err < 0) {
		fprintf(stderr, "ti_rpmsg_send: send failed: %d (%s)\n",
			errno, strerror(errno));
		return -1;
	}
	return 0;
}

/*!
 *  @brief ti_rpmsg_recv: receive message through socket
 */

int ti_rpmsg_recv(int sock, void *msg, int *byteCount)
{
	int status = ECH0_TEST_SUCCESS;
	struct sockaddr_rpmsg fromAddr;  /* [Socket address of sender] */
	unsigned int  len;

	memset(&fromAddr, 0, sizeof (fromAddr));
	len = sizeof (fromAddr);

	*byteCount = recvfrom(sock, msg, MESSAGEQ_RPMSG_MAXSIZE, 0,
			      (struct sockaddr *)&fromAddr, &len);
	if (len != sizeof (fromAddr)) {
		fprintf(stderr, "recvfrom: got bad addr len (%d)\n", len);
		status = ECHO_TEST_ERR_RECV_FAIL;
		goto exit;
	}
	if (*byteCount < 0) {
		fprintf(stderr, "recvfrom: got bad addr len (%d)\n", len);
		status = ECHO_TEST_ERR_RECV_SIZE_FAIL;		
	}

exit:
	return status;
}

/*!
 *  @brief ConnectSocket: Connect TX socket to destination address
 */

int ConnectSocket(int sock, uint16_t procId, int dst)
{
	int                   err;
	struct sockaddr_rpmsg srcAddr, dstAddr;
	socklen_t             len;
	int vproc_id;

	/* TODO: Add table conversion for rpoc to vproc
	   Need to handle platform specific part properly */
	if(procId == REMOTE_RPROC_ID) {
		vproc_id = REMOTE_VPROC_ID;
	} else {
		return -1;
	}
	/* connect to remote service */
	memset(&dstAddr, 0, sizeof(dstAddr));
	dstAddr.family     = AF_RPMSG;
	/* convert MultiProc 'clusterId' to remoteproc index */
	dstAddr.vproc_id   = vproc_id;
	dstAddr.addr       = dst;

	len = sizeof(struct sockaddr_rpmsg);
	err = connect(sock, (struct sockaddr *)&dstAddr, len);
	if (err < 0) {
		/* don't hard-printf since this is no longer fatal */
		fprintf(stderr, "connect failed: %s (%d)\n", strerror(errno), errno);
		return (-1);
	}

	/* let's see what local address we got */
	err = getsockname(sock, (struct sockaddr *)&srcAddr, &len);
	if (err < 0) {
		fprintf(stderr, "getpeername failed: %s (%d)\n",
			strerror(errno), errno);
		return (-1);
	}
	printf("Connected over sock: %d\n\tdst vproc_id: %d, dst addr: %d\n",
	       sock, dstAddr.vproc_id, dstAddr.addr);
	printf("\tsrc vproc_id: %d, src addr: %d\n",
	       srcAddr.vproc_id, srcAddr.addr);
	return 0;
}

/*!
 *  @brief SocketBindAddr: Bind RX socket to local address
 */

int SocketBindAddr(int fd, uint16_t procId, uint32_t localAddr)
{
	int         err;
	socklen_t    len;
	struct sockaddr_rpmsg srcAddr;
	int vproc_id;


	/* map procId to vproc_id */
	if(procId == REMOTE_RPROC_ID) {
		vproc_id = REMOTE_VPROC_ID;
	} else {
		return -1;
	}

	/* Now bind to the source address.   */
	memset(&srcAddr, 0, sizeof(srcAddr));
	srcAddr.family = AF_RPMSG;
	/* We bind the remote clusterId, but local address! */
	srcAddr.vproc_id   = vproc_id;
	srcAddr.addr  = localAddr;

	len = sizeof(struct sockaddr_rpmsg);
	err = bind(fd, (struct sockaddr *)&srcAddr, len);
	if (err >= 0) {
		printf("socket_bind_addr: bound sock: %d\n\tto dst "
		       "vproc_id: %d, src addr: %d\n",
		       fd, srcAddr.vproc_id, srcAddr.addr);

		/* let's see what local address we got */
		err = getsockname(fd, (struct sockaddr *)&srcAddr, &len);
		if (err < 0) {
			fprintf(stderr, "getsockname failed: %s (%d)\n",
				strerror(errno), errno);
		}
		else {
			printf("\tsrc vproc_id: %d, src addr: %d\n",
			       srcAddr.vproc_id, srcAddr.addr);
		}
	}
	return (err);
}

/*!
 *  @brief main: Main test code
 */

void main(void)
{

	int sock=0;
	int status;
	int flags;
	MessageQ_Msg msg=NULL, rcv_msg=NULL;
	Init_MsgHeader init_msg;
	int i, j;
	int recv_sock=0;
	struct sockaddr_rpmsg srcAddr;
	int msgSize, rcvMsgSize;
	uint32_t *dataPtr;

	printf("Start of test");
	/* Create socket for sending messages to remote processor */
	sock = socket(AF_RPMSG, SOCK_SEQPACKET, 0);

	if (sock < 0) {
		status = ECHO_TEST_TX_SOCK_CREATE_FAIL;
		fprintf(stderr,
			"main: socket failed: error %d (%s)\n", errno,
			strerror(errno));
		goto done;
	}
	printf("Tx Socket create success !!\n");

	/* Connect socket to Msq RPMSG port */
	status = ConnectSocket(sock, REMOTE_RPROC_ID, MESSAGEQ_RPMSG_PORT);

	if (status < 0) {
		status = ECHO_TEST_TX_SOCK_CONNECT_FAIL;
		fprintf(stderr,
			"main: connect failed: error %d (%s) procId: %d\n",
			errno, strerror(errno), REMOTE_RPROC_ID);
		goto done;
	}
	printf("Connect Tx Socket success !!\n");

	/* Make sure socket fd doesn't exist for 'fork() -> exec*()'ed child */
	flags = fcntl(sock, F_GETFD);
	if (flags != -1) {
		fcntl(sock, F_SETFD, flags | FD_CLOEXEC);
	}

	/*  Create the socket to receive messages */
	recv_sock = socket(AF_RPMSG, SOCK_SEQPACKET, 0);
	if (recv_sock < 0) {
		fprintf(stderr, "TransportRpmsg_bind: socket call failed: %d (%s)\n",
			errno, strerror(errno));
		status = ECHO_TEST_RX_SOCK_CREATE_FAIL;
		goto done;
	}
	printf("Rx Socket create success !!\n");

	/* Bind socket to Recieve port address */
	status = SocketBindAddr(recv_sock, REMOTE_RPROC_ID, RECEIVE_PORT_ADDR );
	if (status < 0) {
		/* don't hard-printf since this is no longer fatal */
		fprintf(stderr, "TransportRpmsg_bind: bind failed: %d (%s)\n",
		       errno, strerror(errno));
		status = ECHO_TEST_RX_SOCK_BIND_FAIL;
		goto done;
	}

	/* Make sure socket fd doesn't exist for 'fork() -> exec*()'ed child */
	flags = fcntl(recv_sock, F_GETFD);
	if (flags != -1) {
		fcntl(recv_sock, F_SETFD, flags | FD_CLOEXEC);
	}

	printf("Socket Bind success !!\n");

	memset(&init_msg,  0, sizeof(init_msg));
	msgSize = sizeof(init_msg);

	for(i=0; i< 2; i++) {
		/* Send init message twice to prime*/
		status = ti_rpmsg_send(sock, &init_msg, msgSize);
		if (status < 0) {
			fprintf(stderr,
				"main: ti_rpmsg_send %d failed %d (%s)\n", i, errno,
				strerror(errno));
			goto done;
		}
		printf("rpmsg send init message:%d success; size %d!!\n", i, msgSize);
	}

	/* Delay a bit for the init messages to be consumed */
	sleep(1);

	msg = malloc(MESSAGEQ_RPMSG_MAXSIZE * sizeof(char));
	if (!msg) {
		status = ECHO_TEST_ERR_MEMORY;
		goto done ;
	}

	msgSize = MESSAGEQ_RPMSG_MAXSIZE-sizeof(MessageQ_MsgHeader);
	msg->msgSize = MESSAGEQ_RPMSG_MAXSIZE-sizeof(MessageQ_MsgHeader);
	msg->flags = 0;
	msg->msgId = 0;
	msg->dstId = REMOTE_RPROC_ID;
	msg->dstProc = 0;
	msg->replyId = 1;
	msg->replyProc = 1;
	msg->srcProc = 0;
	msg->heapId = 0;
	msg->seqNum = 0;
	dataPtr = (uint32_t *)((char *)(msg) +sizeof(MessageQ_MsgHeader));

	for(i=0; i< 1; i++) {
		msg->seqNum = i;
	 	for(j=0; j<msgSize>>2; j++) {
			dataPtr[j] = j+msg->seqNum;
		}

		status = ti_rpmsg_send(sock, msg, msgSize);
		if (status < 0) {
			fprintf(stderr,
				"main: ti_rpmsg_send %d failed %d (%s)\n", i, errno,
				strerror(errno));
			goto done;
		}
		fprintf(stderr, "ti_rpmsg_send:%d success! Size %d!!\n", i, msgSize);
	}
	/*
	 * We have no way of peeking to see what message size we'll get, so we
	 * allocate a message of max size to receive contents from the rpmsg socket
	 * (currently, a copy transport)
	 */
	rcv_msg = malloc(MESSAGEQ_RPMSG_MAXSIZE*sizeof(char));
	if (!rcv_msg) {
		status = ECHO_TEST_ERR_MEMORY;
		goto done;
	}

	do {
		/* Receive message */
		status = ti_rpmsg_recv(recv_sock, rcv_msg, &rcvMsgSize);
		if (status < 0) {
			fprintf(stderr, "ti_rpmsg_recv failed! status %d, rcvMsgSize %d\n",
				status, rcvMsgSize);
			goto done;
		}
		if (rcvMsgSize < 16) {
			/* Ignore any init messages returned */
			printf("ti_rpmsg_recv: receive dummy message success size %d!!\n",
			       rcvMsgSize);
		} else {
			printf("ti_rpmsg_recv: receive success! Size %d!!\n", rcvMsgSize);
		}
	} while (rcvMsgSize < 16);

#ifdef DEBUG
	for (i=0; i<rcvMsgSize; i+=4) {
		printf("%x %x %x %x\n",
		       *(((uint8_t *)(rcv_msg))+i),
		       *(((uint8_t *)(rcv_msg))+i+1),
		       *(((uint8_t *)(rcv_msg))+i+2),
		       *(((uint8_t *)(rcv_msg))+i+3)
		       );
	}
#endif
	printf("Check receive msg complete !!\n");
	status = memcmp(rcv_msg, msg, rcvMsgSize);
	if(status) {
		fprintf(stderr, "Message mismatch %d !!\n", status);
		status = ECHO_TEST_MESSAGE_MISMATCH;
		goto done;
	}
	printf("Test Pass: Messages match size %d, status %d !!\n", rcvMsgSize, status);

done:
	if (recv_sock)
		close(recv_sock);
	if (sock)
		close(sock);
	if(rcv_msg)
		free(rcv_msg);
	if(msg)
		free(msg);
}