diff --git a/mcs/openamp_demo_proc_ptyshell/Makefile b/mcs/openamp_demo_proc_ptyshell/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..5907eef3d33477956c9caccd9ba92cf3783c9521 --- /dev/null +++ b/mcs/openamp_demo_proc_ptyshell/Makefile @@ -0,0 +1,14 @@ +rpmsg_main: rpmsg_main.o rpmsg_linux_endpoint.o rpmsg_rtos_endpoint.o + $(CC) rpmsg_main.o rpmsg_linux_endpoint.o rpmsg_rtos_endpoint.o -g $(SDKTARGETSYSROOT)/usr/lib64/libmetal.so $(SDKTARGETSYSROOT)/usr/lib64/libopen_amp.so $(SDKTARGETSYSROOT)/lib64/libsysfs.so -o rpmsg_main + +rpmsg_main.o: rpmsg_main.c rpmsg-internal.h + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_main.c -o rpmsg_main.o + +rpmsg_linux_endpoint.o: rpmsg_linux_endpoint.c rpmsg-internal.h + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_linux_endpoint.c -o rpmsg_linux_endpoint.o + +rpmsg_rtos_endpoint.o: rpmsg_rtos_endpoint.c + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_rtos_endpoint.c -o rpmsg_rtos_endpoint.o + +clean: + rm -rf *.o rpmsg_main diff --git a/mcs/openamp_demo_proc_ptyshell/rpmsg-internal.h b/mcs/openamp_demo_proc_ptyshell/rpmsg-internal.h new file mode 100644 index 0000000000000000000000000000000000000000..82ce62eb0d5b59d1b729ff4a9515c56fe3c77b9d --- /dev/null +++ b/mcs/openamp_demo_proc_ptyshell/rpmsg-internal.h @@ -0,0 +1,55 @@ +#ifndef RPMSG_INTERNAL_H_ +#define RPMSG_INTERNAL_H_ + +#include +#include +#include + +#define SHM_START_ADDR 0x70000000 +#define SHM_SIZE 0x30000 + +#define VRING_ADDR_SIZE 0x4000 +#define VRING_RX_ADDRESS (SHM_START_ADDR + SHM_SIZE) +#define VRING_TX_ADDRESS (VRING_RX_ADDRESS + VRING_ADDR_SIZE) +#define RTOS_VRING_RX_ADDRESS VRING_TX_ADDRESS +#define RTOS_VRING_TX_ADDRESS VRING_RX_ADDRESS + +#define VRING_COUNT 2 +#define VRING_ALIGNMENT 4 +#define VRING_SIZE 16 + +#define RSC_TABLE_ADDR 0x71000000 + +#define VDEV_NOTIFY_ID 0 +#define VRING_RX_NOTIFY_ID 1 +#define VRING_TX_NOTIFY_ID 2 + +struct rproc_priv { + struct remoteproc *rproc; + unsigned int id; +}; + +#define NO_RESOURCE_ENTRIES 8 + +/* Resource table for the given remote */ +struct remote_resource_table { + unsigned int version; + unsigned int num; + unsigned int reserved[2]; + unsigned int offset[NO_RESOURCE_ENTRIES]; + + /* 1. rpmsg buffer mem entry */ + struct fw_rsc_carveout rpmsg_mem; + + /* 2. rpmsg vdev entry */ + struct fw_rsc_vdev rpmsg_vdev; + struct fw_rsc_vdev_vring rpmsg_vring0; + struct fw_rsc_vdev_vring rpmsg_vring1; +} __attribute__((__packed__)); + +extern int pipefd1[2]; +extern int pipefd2[2]; + +void *get_resource_table(int *len); + +#endif diff --git a/mcs/openamp_demo_proc_ptyshell/rpmsg_linux_endpoint.c b/mcs/openamp_demo_proc_ptyshell/rpmsg_linux_endpoint.c new file mode 100644 index 0000000000000000000000000000000000000000..7d56db467da11c9d5b197f39e4a248ab4a136135 --- /dev/null +++ b/mcs/openamp_demo_proc_ptyshell/rpmsg_linux_endpoint.c @@ -0,0 +1,332 @@ +/* Linux Endpoint run in parent process, isolate with child process */ + +#include +#define __USE_XOPEN_EXTENDED +#define __USE_XOPEN2KXSI +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +static volatile unsigned int received_data; +static struct rpmsg_virtio_shm_pool shpool; //tmp, must be global var +static struct rpmsg_endpoint linux_ept; //tmp, must be global var + +static struct remoteproc *rproc_init(struct remoteproc *rproc, + struct remoteproc_ops *ops, void *arg) +{ + struct rproc_priv *priv; + unsigned int id = *((unsigned int *)arg); + + priv = metal_allocate_memory(sizeof(*priv)); + if (!priv) + return NULL; + + memset(priv, 0, sizeof(*priv)); + priv->rproc = rproc; + priv->id = id; + priv->rproc->ops = ops; + metal_list_init(&priv->rproc->mems); + priv->rproc->priv = priv; + rproc->state = RPROC_READY; + return priv->rproc; +} + +static void rproc_remove(struct remoteproc *rproc) +{ + struct rproc_priv *priv; + + priv = (struct rproc_priv *)rproc->priv; + metal_free_memory(priv); +} + +static void *rproc_mmap(struct remoteproc *rproc, + metal_phys_addr_t *pa, metal_phys_addr_t *da, + size_t size, unsigned int attribute, + struct metal_io_region **io) +{ + struct remoteproc_mem *mem; + struct metal_io_region *tmpio; + + if (*pa == METAL_BAD_PHYS && *da == METAL_BAD_PHYS) + return NULL; + if (*pa == METAL_BAD_PHYS) + *pa = *da; + if (*da == METAL_BAD_PHYS) + *da = *pa; + + mem = metal_allocate_memory(sizeof(*mem)); + if (!mem) + return NULL; + tmpio = metal_allocate_memory(sizeof(*tmpio)); + if (!tmpio) { + metal_free_memory(mem); + return NULL; + } + + /* mmap pa to va */ + int memfd; + void *va; + memfd = open("/dev/mem", O_RDWR); + va = mmap((void*)pa, size, PROT_READ | PROT_WRITE, MAP_SHARED, memfd, *pa); + + remoteproc_init_mem(mem, NULL, *pa, *da, size, tmpio); + metal_io_init(tmpio, va, &mem->pa, size, -1, attribute, NULL); /* metal io manage va and pa */ + remoteproc_add_mem(rproc, mem); + if (io) + *io = tmpio; + + printf("master mmap pa=0x%lx, da=0x%lx, size=0x%lx, attribute=0x%x va=%p\n", *pa, *da, size, attribute, va); + + return metal_io_phys_to_virt(tmpio, mem->pa); +} + +//master tmp +static int rproc_notify_tmp(struct remoteproc *rproc, uint32_t id) +{ + return 0; +} + +static int rproc_notify(struct remoteproc *rproc, uint32_t id) +{ + int ret; + + printf("master notify start\n"); + + /* notify RTOS Endpoint using IPC */ + ret = write(pipefd1[1], "Linux: notify ipi\n", strlen("Linux: notify ipi\n")); + if (ret == -1) + perror("master write pipefd1[1]"); + + return 0; +} + +static struct remoteproc_ops rproc_ops = { + .init = rproc_init, + .remove = rproc_remove, + .mmap = rproc_mmap, + .notify = rproc_notify_tmp, //master tmp +}; + +static unsigned int receive_message(struct remoteproc *rproc) +{ + char buf[50] = {0}; + int ret; + + /* 1. poll and wait for IPI(IPC) from RTOS Endpoint */ + while (read(pipefd2[0], buf, sizeof(buf)) > 0) { + printf("master poll:%s", buf); + if (strcmp(buf, "RTOS: notify ipi\n") == 0) + break; + } + + /* 2. receive data */ + ret = remoteproc_get_notification(rproc, VRING_RX_NOTIFY_ID); + if (ret) + printf("remoteproc_get_notification failed: 0x%x\n", ret); + return received_data; +} + +static int endpoint_cb(struct rpmsg_endpoint *ept, void *data, + size_t len, uint32_t src, void *priv) +{ + received_data = *((unsigned int *) data); + return RPMSG_SUCCESS; +} + +static void rpmsg_service_unbind(struct rpmsg_endpoint *ept) +{ + rpmsg_destroy_ept(ept); +} + +static void ns_bind_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest) +{ + (void)rpmsg_create_ept(&linux_ept, rdev, name, + RPMSG_ADDR_ANY, dest, + endpoint_cb, + rpmsg_service_unbind); +} + +static int send_message(unsigned int message, struct rpmsg_endpoint *ept) +{ + return rpmsg_send(ept, &message, sizeof(message)); +} + +static struct remoteproc *platform_create_proc(unsigned int id) +{ + struct remoteproc *rproc; + struct remoteproc rproc_inst; + void *va; + void *rsc; + int ret; + metal_phys_addr_t pa = RSC_TABLE_ADDR; + int rsc_size; + + /* Initialize remoteproc instance */ + rproc = remoteproc_init(&rproc_inst, &rproc_ops, &id); + if (!rproc) + return NULL; + + /* mmap resource table */ + rsc = get_resource_table(&rsc_size); + va = remoteproc_mmap(rproc, &pa, NULL, rsc_size, 0, NULL); + memcpy(va, rsc, rsc_size); + + /* parse resource table to remoteproc */ + ret = remoteproc_set_rsc_table(rproc, va, rsc_size); + if (ret) { + printf("Failed to initialize remoteproc, ret:0x%x\n", ret); + remoteproc_remove(rproc); + return NULL; + } + + //tmp + struct remote_resource_table *va_tmp = va; + struct remote_resource_table *rsc_tmp = rsc; + va_tmp->rpmsg_vdev.notifyid = rsc_tmp->rpmsg_vdev.notifyid; + va_tmp->rpmsg_vdev.vring[0].notifyid = rsc_tmp->rpmsg_vdev.vring[0].notifyid; + va_tmp->rpmsg_vdev.vring[1].notifyid = rsc_tmp->rpmsg_vdev.vring[1].notifyid; + + printf("(1)master platform_create_proc success\n"); + + return rproc; +} + +static struct rpmsg_device *platform_create_rpmsg_vdev(struct remoteproc *rproc) +{ + struct rpmsg_virtio_device rpmsg_vdev; + struct virtio_device *vdev; + void *shbuf; + struct metal_io_region *shbuf_io; + int ret; + + shbuf_io = remoteproc_get_io_with_pa(rproc, SHM_START_ADDR); + if (!shbuf_io) + return NULL; + shbuf = metal_io_phys_to_virt(shbuf_io, SHM_START_ADDR); + + printf("(2)master creating remoteproc virtio\n"); + vdev = remoteproc_create_virtio(rproc, 0, RPMSG_MASTER, NULL); + if (!vdev) { + printf("failed remoteproc_create_virtio\n"); + return NULL; + } + + printf("(3)master initializing rpmsg shared buffer pool\n"); + /* Only RPMsg virtio master needs to initialize the shared buffers pool */ + rpmsg_virtio_init_shm_pool(&shpool, shbuf, SHM_SIZE); + + printf("(4)master initializing rpmsg vdev\n"); + /* RPMsg virtio slave can set shared buffers pool argument to NULL */ + ret = rpmsg_init_vdev(&rpmsg_vdev, vdev, ns_bind_cb, shbuf_io, &shpool); + if (ret) { + printf("failed rpmsg_init_vdev\n"); + remoteproc_remove_virtio(rproc, vdev); + return NULL; + } + + //printf("master vq[0].callback:%p, vq[1].callback:%p\n", vdev->vrings_info[0].vq->callback, vdev->vrings_info[1].vq->callback); + + printf("(5)master returning rdev\n"); + return rpmsg_virtio_get_rpmsg_device(&rpmsg_vdev); +} + +static void rpmsg_app_master(struct remoteproc *rproc, int fdm) +{ + int status = 0; + unsigned int message; + + //tmp + /* register notify function */ + rproc->ops->notify = rproc_notify; + + /* 1.2 master receive NS Announcement, and process it */ + receive_message(rproc); + + printf("1.2 master receive_message succeed\n"); + printf("1.2 master Endpoint: name:%s, addr:0x%x, dest_addr:0x%x\n", linux_ept.name, linux_ept.addr, linux_ept.dest_addr); + + char buffer[256] = {0}; + while (read(fdm, buffer, sizeof(buffer)) > 0) { + message = atoi(buffer); + /* 2. master send message */ + status = send_message(message, &linux_ept); + if (status < 0) + printf("send_message(%u) failed with status %d\n", message, status); + printf("2. master send_message succeed\n"); + + /* 5. master receive message */ + message = receive_message(rproc); + printf("5. Master core received a message: %u\n\n", message); + snprintf(buffer, sizeof(buffer), "%d\n", message); + status = write(fdm, buffer, strlen(buffer)); + if (status == -1) + perror("master write fdm"); + } +} + +void open_pty(int *pfdm, int *pfds) +{ + int ret, fdm, fds; + struct termios tty = {0}; + + /* Open the master side of the PTY */ + fdm = posix_openpt(O_RDWR | O_NOCTTY); + if (fdm < 0) + printf("Error %d on posix_openpt()\n", errno); + + ret = grantpt(fdm); + if (ret != 0) + printf("Error %d on grantpt()\n", errno); + + ret = unlockpt(fdm); + if (ret != 0) + printf("Error %d on unlockpt()\n", errno); + + /* Open the slave side of the PTY */ + fds = open(ptsname(fdm), O_RDWR | O_NOCTTY); + + /* Set the state of fds to TERMIOS_P */ + tty.c_cflag = CS8; + tcsetattr(fds, TCSAFLUSH, &tty); + + *pfdm = fdm; + *pfds = fds; +} + +int rpmsg_linux_endpoint() +{ + struct remoteproc *rproc = NULL; + unsigned int id = 1; + struct rpmsg_device *rdev = NULL; + int fdm, fds; + + open_pty(&fdm, &fds); + + /* create remoteproc */ + rproc = platform_create_proc(id); + if (!rproc) { + printf("create rproc failed\n"); + return -1; + } + + rdev = platform_create_rpmsg_vdev(rproc); + if (!rdev) + printf("master create rpmsg vdev failed\n"); + + rpmsg_app_master(rproc, fdm); + //remoteproc_remove(rproc); + + return 0; +} diff --git a/mcs/openamp_demo_proc_ptyshell/rpmsg_main.c b/mcs/openamp_demo_proc_ptyshell/rpmsg_main.c new file mode 100644 index 0000000000000000000000000000000000000000..ac08fba1f902f95cd535c2c9ea6a52bfd6ef22bd --- /dev/null +++ b/mcs/openamp_demo_proc_ptyshell/rpmsg_main.c @@ -0,0 +1,76 @@ +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +int pipefd1[2]; +int pipefd2[2]; + +/* Place resource table in special ELF section */ +#define __section_t(S) __attribute__((__section__(#S))) +#define __resource __section_t(.resource_table) + +#define NUM_TABLE_ENTRIES 2 +#define DFEATURE_SUPPORT_NS 1 + +struct remote_resource_table __resource resources = { + /* Version */ + RSC_TAB_SUPPORTED_VERSION, + /* NUmber of table entries */ + NUM_TABLE_ENTRIES, + /* reserved fields */ + { 0, 0 }, + /* Offsets of rsc entries */ + { + offsetof(struct remote_resource_table, rpmsg_mem), + offsetof(struct remote_resource_table, rpmsg_vdev), + }, + + /* 1. Rpmsg buffer memory entry including vring TX/RX memory */ + { RSC_CARVEOUT, SHM_START_ADDR, SHM_START_ADDR, SHM_SIZE, 0 }, + + /* 2. Virtio device entry */ + { RSC_VDEV, VIRTIO_ID_RPMSG, VDEV_NOTIFY_ID, DFEATURE_SUPPORT_NS, 0, 0, 0, VRING_COUNT, { 0, 0 } }, + /* Vring rsc entry - part of vdev rsc entry */ + { VRING_RX_ADDRESS, VRING_ALIGNMENT, VRING_SIZE, VRING_RX_NOTIFY_ID, 0 }, /* VRING_RX */ + { VRING_TX_ADDRESS, VRING_ALIGNMENT, VRING_SIZE, VRING_TX_NOTIFY_ID, 0 }, /* VRING_TX */ +}; + +void *get_resource_table(int *len) +{ + *len = sizeof(resources); + return &resources; +} + +extern int rpmsg_linux_endpoint(); +extern int rpmsg_rtos_endpoint(); + +int main(int argc, char **argv) +{ + pid_t pid; + + /* Create unamed pipe */ + if (pipe(pipefd1) < 0) { + perror("pipe1"); + return -1; + } + if (pipe(pipefd2) < 0) { + perror("pipe2"); + return -1; + } + + if ((pid = fork()) < 0) { + perror("fork"); + return -1; + } else if (pid > 0) { + /* parent: Linux Endpoint */ + rpmsg_linux_endpoint(); + } else { + /* child: RTOS Endpoint */ + rpmsg_rtos_endpoint(); + } + + return 0; +} diff --git a/mcs/openamp_demo_proc_ptyshell/rpmsg_rtos_endpoint.c b/mcs/openamp_demo_proc_ptyshell/rpmsg_rtos_endpoint.c new file mode 100644 index 0000000000000000000000000000000000000000..7094b436f86ed42e94bf38fd384e533c341826ec --- /dev/null +++ b/mcs/openamp_demo_proc_ptyshell/rpmsg_rtos_endpoint.c @@ -0,0 +1,272 @@ +/* RTOS Endpoint run in child process, isolate with parent process */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +static volatile unsigned int rtos_received_data; + +static struct remoteproc *rtos_rproc_init(struct remoteproc *rproc, + struct remoteproc_ops *ops, void *arg) +{ + struct rproc_priv *priv; + unsigned int id = *((unsigned int *)arg); + + priv = metal_allocate_memory(sizeof(*priv)); + if (!priv) + return NULL; + + memset(priv, 0, sizeof(*priv)); + priv->rproc = rproc; + priv->id = id; + priv->rproc->ops = ops; + metal_list_init(&priv->rproc->mems); + priv->rproc->priv = priv; + rproc->state = RPROC_READY; + return priv->rproc; +} + +static void rtos_rproc_remove(struct remoteproc *rproc) +{ + struct rproc_priv *priv; + + priv = (struct rproc_priv *)rproc->priv; + metal_free_memory(priv); +} + +static void *rtos_rproc_mmap(struct remoteproc *rproc, + metal_phys_addr_t *pa, metal_phys_addr_t *da, + size_t size, unsigned int attribute, + struct metal_io_region **io) +{ + struct remoteproc_mem *mem; + struct metal_io_region *tmpio; + + if (*pa == METAL_BAD_PHYS && *da == METAL_BAD_PHYS) + return NULL; + if (*pa == METAL_BAD_PHYS) + *pa = *da; + if (*da == METAL_BAD_PHYS) + *da = *pa; + + mem = metal_allocate_memory(sizeof(*mem)); + if (!mem) + return NULL; + tmpio = metal_allocate_memory(sizeof(*tmpio)); + if (!tmpio) { + metal_free_memory(mem); + return NULL; + } + + /* mmap pa to va */ + int memfd; + void *va; + memfd = open("/dev/mem", O_RDWR); + va = mmap((void*)pa, size, PROT_READ | PROT_WRITE, MAP_SHARED, memfd, *pa); + + remoteproc_init_mem(mem, NULL, *pa, *da, size, tmpio); + metal_io_init(tmpio, va, &mem->pa, size, -1, attribute, NULL); /* metal io manage va and pa */ + remoteproc_add_mem(rproc, mem); + if (io) + *io = tmpio; + + printf("slave mmap pa=0x%lx, da=0x%lx, size=0x%lx, attribute=0x%x va=%p\n", *pa, *da, size, attribute, va); + + return metal_io_phys_to_virt(tmpio, mem->pa); +} + +static int rtos_rproc_notify(struct remoteproc *rproc, uint32_t id) +{ + int ret; + + printf("slave notify start\n"); + + /* notify RTOS Endpoint using IPC */ + ret = write(pipefd2[1], "RTOS: notify ipi\n", strlen("RTOS: notify ipi\n")); + if (ret == -1) + perror("slave write pipefd2[1]"); + + return 0; +} + +static struct remoteproc_ops rtos_rproc_ops = { + .init = rtos_rproc_init, + .remove = rtos_rproc_remove, + .mmap = rtos_rproc_mmap, + .notify = rtos_rproc_notify, +}; + +static unsigned int rtos_receive_message(struct remoteproc *rproc) +{ + char buf[50] = {0}; + int ret; + + /* 1. poll and wait for IPI(IPC) from RTOS Endpoint */ + while (read(pipefd1[0], buf, sizeof(buf)) > 0) { + printf("slave poll:%s", buf); + if (strcmp(buf, "Linux: notify ipi\n") == 0) + break; + } + + /* 2. receive data */ + ret = remoteproc_get_notification(rproc, VRING_TX_NOTIFY_ID); + if (ret) + printf("remoteproc_get_notification failed: 0x%x\n", ret); + return rtos_received_data; +} + +static int rtos_endpoint_cb(struct rpmsg_endpoint *ept, void *data, + size_t len, uint32_t src, void *priv) +{ + rtos_received_data = *((unsigned int *) data); + return RPMSG_SUCCESS; +} + +static void rtos_rpmsg_service_unbind(struct rpmsg_endpoint *ept) +{ + rpmsg_destroy_ept(ept); +} + +static int rtos_send_message(unsigned int message, struct rpmsg_endpoint *ept) +{ + return rpmsg_send(ept, &message, sizeof(message)); +} + +static struct remoteproc *rtos_platform_create_proc(unsigned int id) +{ + struct remoteproc *rproc; + struct remoteproc rproc_inst; + void *va; + void *rsc; + int ret; + metal_phys_addr_t pa = RSC_TABLE_ADDR; + int rsc_size; + + /* Initialize remoteproc instance */ + rproc = remoteproc_init(&rproc_inst, &rtos_rproc_ops, &id); + if (!rproc) + return NULL; + + /* mmap resource table */ + rsc = get_resource_table(&rsc_size); + va = remoteproc_mmap(rproc, &pa, NULL, rsc_size, 0, NULL); + memcpy(va, rsc, rsc_size); + + /* parse resource table to remoteproc */ + ret = remoteproc_set_rsc_table(rproc, va, rsc_size); + if (ret) { + printf("Failed to initialize remoteproc, ret:0x%x\n", ret); + remoteproc_remove(rproc); + return NULL; + } + + //tmp + struct remote_resource_table *va_tmp = va; + struct remote_resource_table *rsc_tmp = rsc; + va_tmp->rpmsg_vdev.notifyid = rsc_tmp->rpmsg_vdev.notifyid; + va_tmp->rpmsg_vdev.vring[0].notifyid = rsc_tmp->rpmsg_vdev.vring[0].notifyid; + va_tmp->rpmsg_vdev.vring[1].notifyid = rsc_tmp->rpmsg_vdev.vring[1].notifyid; + + printf("(1)slave platform_create_proc success\n"); + + return rproc; +} + +static struct rpmsg_device *rtos_platform_create_rpmsg_vdev(struct remoteproc *rproc) +{ + struct rpmsg_virtio_device rpmsg_vdev; + struct virtio_device *vdev; + void *shbuf; + struct metal_io_region *shbuf_io; + int ret; + + shbuf_io = remoteproc_get_io_with_pa(rproc, SHM_START_ADDR); + if (!shbuf_io) + return NULL; + shbuf = metal_io_phys_to_virt(shbuf_io, SHM_START_ADDR); + + printf("(2)slave creating remoteproc virtio\n"); + vdev = remoteproc_create_virtio(rproc, 0, RPMSG_REMOTE, NULL); + if (!vdev) { + printf("failed remoteproc_create_virtio\n"); + return NULL; + } + + printf("(3)slave initializing rpmsg vdev\n"); + /* RPMsg virtio slave can set shared buffers pool argument to NULL */ + ret = rpmsg_init_vdev(&rpmsg_vdev, vdev, NULL, shbuf_io, NULL); + if (ret) { + printf("failed rpmsg_init_vdev\n"); + remoteproc_remove_virtio(rproc, vdev); + return NULL; + } + + //printf("slave vq[0].callback:%p, vq[1].callback:%p\n", vdev->vrings_info[0].vq->callback, vdev->vrings_info[1].vq->callback); + + printf("(4)slave returning rdev\n"); + return rpmsg_virtio_get_rpmsg_device(&rpmsg_vdev); +} + +static void rtos_rpmsg_app_master(struct remoteproc *rproc, struct rpmsg_device *rdev) +{ + int status = 0; + unsigned int message; + struct rpmsg_endpoint ept; + + /* 1.1 slave create endpoint, and send an NS Announcement */ + status = rpmsg_create_ept(&ept, rdev, "k", RPMSG_ADDR_ANY, RPMSG_ADDR_ANY, rtos_endpoint_cb, rtos_rpmsg_service_unbind); + if (status < 0) + printf("slave rpmsg_create_ept failed:%d\n", status); + + printf("1.1 slave rpmsg_create_ept succeed\n"); + printf("1.1 slave Endpoint: name:%s, addr:0x%x, dest_addr:0x%x\n", ept.name, ept.addr, ept.dest_addr); + + while (1) { + /* 3. slave receive message */ + message = rtos_receive_message(rproc); + printf("3. Slave core received a message: %u\n", message); + message++; + printf("3. slave Endpoint: name:%s, addr:0x%x, dest_addr:0x%x\n", ept.name, ept.addr, ept.dest_addr); + + /* 4. slave send message */ + status = rtos_send_message(message, &ept); + if (status < 0) + printf("rtos_send_message(%u) failed with status %d\n", message, status); + printf("4. slave rtos_send_message succeed\n"); + } +} + +int rpmsg_rtos_endpoint() +{ + struct remoteproc *rproc = NULL; + unsigned int id = 1; + struct rpmsg_device *rdev = NULL; + + /* create remoteproc */ + rproc = rtos_platform_create_proc(id); + if (!rproc) { + printf("create rproc failed\n"); + return -1; + } + + rdev = rtos_platform_create_rpmsg_vdev(rproc); + if (!rdev) + printf("slave create rpmsg vdev failed\n"); + + rtos_rpmsg_app_master(rproc, rdev); + //remoteproc_remove(rproc); + + return 0; +} diff --git a/mcs/openamp_demo_proc_rsctbl/Makefile b/mcs/openamp_demo_proc_rsctbl/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..5907eef3d33477956c9caccd9ba92cf3783c9521 --- /dev/null +++ b/mcs/openamp_demo_proc_rsctbl/Makefile @@ -0,0 +1,14 @@ +rpmsg_main: rpmsg_main.o rpmsg_linux_endpoint.o rpmsg_rtos_endpoint.o + $(CC) rpmsg_main.o rpmsg_linux_endpoint.o rpmsg_rtos_endpoint.o -g $(SDKTARGETSYSROOT)/usr/lib64/libmetal.so $(SDKTARGETSYSROOT)/usr/lib64/libopen_amp.so $(SDKTARGETSYSROOT)/lib64/libsysfs.so -o rpmsg_main + +rpmsg_main.o: rpmsg_main.c rpmsg-internal.h + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_main.c -o rpmsg_main.o + +rpmsg_linux_endpoint.o: rpmsg_linux_endpoint.c rpmsg-internal.h + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_linux_endpoint.c -o rpmsg_linux_endpoint.o + +rpmsg_rtos_endpoint.o: rpmsg_rtos_endpoint.c + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_rtos_endpoint.c -o rpmsg_rtos_endpoint.o + +clean: + rm -rf *.o rpmsg_main diff --git a/mcs/openamp_demo_proc_rsctbl/rpmsg-internal.h b/mcs/openamp_demo_proc_rsctbl/rpmsg-internal.h new file mode 100644 index 0000000000000000000000000000000000000000..9caea59987188981e7e7c11d06a084cbdbf24358 --- /dev/null +++ b/mcs/openamp_demo_proc_rsctbl/rpmsg-internal.h @@ -0,0 +1,54 @@ +#ifndef RPMSG_INTERNAL_H_ +#define RPMSG_INTERNAL_H_ + +#include +#include +#include + +#define SHM_START_ADDR 0x70000000 +#define SHM_SIZE 0x30000 + +#define VRING_ADDR_SIZE 0x4000 +#define VRING_RX_ADDRESS (SHM_START_ADDR + SHM_SIZE) +#define VRING_TX_ADDRESS (VRING_RX_ADDRESS + VRING_ADDR_SIZE) +#define RTOS_VRING_RX_ADDRESS VRING_TX_ADDRESS +#define RTOS_VRING_TX_ADDRESS VRING_RX_ADDRESS + +#define VRING_COUNT 2 +#define VRING_ALIGNMENT 4 +#define VRING_SIZE 16 + +#define RSC_TABLE_ADDR 0x71000000 + +#define VDEV_NOTIFY_ID 0 +#define VRING_RX_NOTIFY_ID 1 +#define VRING_TX_NOTIFY_ID 2 + +struct rproc_priv { + struct remoteproc *rproc; + unsigned int id; +}; + +#define NO_RESOURCE_ENTRIES 8 + +/* Resource table for the given remote */ +struct remote_resource_table { + unsigned int version; + unsigned int num; + unsigned int reserved[2]; + unsigned int offset[NO_RESOURCE_ENTRIES]; + + /* 1. rpmsg buffer mem entry */ + struct fw_rsc_carveout rpmsg_mem; + + /* 2. rpmsg vdev entry */ + struct fw_rsc_vdev rpmsg_vdev; + struct fw_rsc_vdev_vring rpmsg_vring0; + struct fw_rsc_vdev_vring rpmsg_vring1; +} __attribute__((__packed__)); + +extern int pipefd[2]; + +void *get_resource_table(int *len); + +#endif diff --git a/mcs/openamp_demo_proc_rsctbl/rpmsg_linux_endpoint.c b/mcs/openamp_demo_proc_rsctbl/rpmsg_linux_endpoint.c new file mode 100644 index 0000000000000000000000000000000000000000..f4463f93bbd25a1dc68e785bb951f2bf99cb4016 --- /dev/null +++ b/mcs/openamp_demo_proc_rsctbl/rpmsg_linux_endpoint.c @@ -0,0 +1,296 @@ +/* Linux Endpoint run in parent process, isolate with child process */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +static volatile unsigned int received_data; +static struct rpmsg_virtio_shm_pool shpool; //tmp, must be global var +static struct rpmsg_endpoint linux_ept; //tmp, must be global var + +static struct remoteproc *rproc_init(struct remoteproc *rproc, + struct remoteproc_ops *ops, void *arg) +{ + struct rproc_priv *priv; + unsigned int id = *((unsigned int *)arg); + + priv = metal_allocate_memory(sizeof(*priv)); + if (!priv) + return NULL; + + memset(priv, 0, sizeof(*priv)); + priv->rproc = rproc; + priv->id = id; + priv->rproc->ops = ops; + metal_list_init(&priv->rproc->mems); + priv->rproc->priv = priv; + rproc->state = RPROC_READY; + return priv->rproc; +} + +static void rproc_remove(struct remoteproc *rproc) +{ + struct rproc_priv *priv; + + priv = (struct rproc_priv *)rproc->priv; + metal_free_memory(priv); +} + +static void *rproc_mmap(struct remoteproc *rproc, + metal_phys_addr_t *pa, metal_phys_addr_t *da, + size_t size, unsigned int attribute, + struct metal_io_region **io) +{ + struct remoteproc_mem *mem; + struct metal_io_region *tmpio; + + if (*pa == METAL_BAD_PHYS && *da == METAL_BAD_PHYS) + return NULL; + if (*pa == METAL_BAD_PHYS) + *pa = *da; + if (*da == METAL_BAD_PHYS) + *da = *pa; + + mem = metal_allocate_memory(sizeof(*mem)); + if (!mem) + return NULL; + tmpio = metal_allocate_memory(sizeof(*tmpio)); + if (!tmpio) { + metal_free_memory(mem); + return NULL; + } + + /* mmap pa to va */ + int memfd; + void *va; + memfd = open("/dev/mem", O_RDWR); + va = mmap((void*)pa, size, PROT_READ | PROT_WRITE, MAP_SHARED, memfd, *pa); + + remoteproc_init_mem(mem, NULL, *pa, *da, size, tmpio); + metal_io_init(tmpio, va, &mem->pa, size, -1, attribute, NULL); /* metal io manage va and pa */ + remoteproc_add_mem(rproc, mem); + if (io) + *io = tmpio; + + printf("master mmap pa=0x%lx, da=0x%lx, size=0x%lx, attribute=0x%x va=%p\n", *pa, *da, size, attribute, va); + + return metal_io_phys_to_virt(tmpio, mem->pa); +} + +//master tmp +static int rproc_notify_tmp(struct remoteproc *rproc, uint32_t id) +{ + return 0; +} + +static int rproc_notify(struct remoteproc *rproc, uint32_t id) +{ + int ret; + + printf("master notify start\n"); + + /* notify RTOS Endpoint using IPC */ + ret = write(pipefd1[1], "Linux: notify ipi\n", strlen("Linux: notify ipi\n")); + if (ret == -1) + perror("master write pipefd1[1]"); + + return 0; +} + +static struct remoteproc_ops rproc_ops = { + .init = rproc_init, + .remove = rproc_remove, + .mmap = rproc_mmap, + .notify = rproc_notify_tmp, //master tmp +}; + +static unsigned int receive_message(struct remoteproc *rproc) +{ + char buf[50] = {0}; + int ret; + + /* 1. poll and wait for IPI(IPC) from RTOS Endpoint */ + while (read(pipefd2[0], buf, sizeof(buf)) > 0) { + printf("master poll:%s", buf); + if (strcmp(buf, "RTOS: notify ipi\n") == 0) + break; + } + + /* 2. receive data */ + ret = remoteproc_get_notification(rproc, VRING_RX_NOTIFY_ID); + if (ret) + printf("remoteproc_get_notification failed: 0x%x\n", ret); + return received_data; +} + +static int endpoint_cb(struct rpmsg_endpoint *ept, void *data, + size_t len, uint32_t src, void *priv) +{ + received_data = *((unsigned int *) data); + return RPMSG_SUCCESS; +} + +static void rpmsg_service_unbind(struct rpmsg_endpoint *ept) +{ + rpmsg_destroy_ept(ept); +} + +static void ns_bind_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest) +{ + (void)rpmsg_create_ept(&linux_ept, rdev, name, + RPMSG_ADDR_ANY, dest, + endpoint_cb, + rpmsg_service_unbind); +} + +static int send_message(unsigned int message, struct rpmsg_endpoint *ept) +{ + return rpmsg_send(ept, &message, sizeof(message)); +} + +static struct remoteproc *platform_create_proc(unsigned int id) +{ + struct remoteproc *rproc; + struct remoteproc rproc_inst; + void *va; + void *rsc; + int ret; + metal_phys_addr_t pa = RSC_TABLE_ADDR; + int rsc_size; + + /* Initialize remoteproc instance */ + rproc = remoteproc_init(&rproc_inst, &rproc_ops, &id); + if (!rproc) + return NULL; + + /* mmap resource table */ + rsc = get_resource_table(&rsc_size); + va = remoteproc_mmap(rproc, &pa, NULL, rsc_size, 0, NULL); + memcpy(va, rsc, rsc_size); + + /* parse resource table to remoteproc */ + ret = remoteproc_set_rsc_table(rproc, va, rsc_size); + if (ret) { + printf("Failed to initialize remoteproc, ret:0x%x\n", ret); + remoteproc_remove(rproc); + return NULL; + } + + //tmp + struct remote_resource_table *va_tmp = va; + struct remote_resource_table *rsc_tmp = rsc; + va_tmp->rpmsg_vdev.notifyid = rsc_tmp->rpmsg_vdev.notifyid; + va_tmp->rpmsg_vdev.vring[0].notifyid = rsc_tmp->rpmsg_vdev.vring[0].notifyid; + va_tmp->rpmsg_vdev.vring[1].notifyid = rsc_tmp->rpmsg_vdev.vring[1].notifyid; + + printf("(1)master platform_create_proc success\n"); + + return rproc; +} + +static struct rpmsg_device *platform_create_rpmsg_vdev(struct remoteproc *rproc) +{ + struct rpmsg_virtio_device rpmsg_vdev; + struct virtio_device *vdev; + void *shbuf; + struct metal_io_region *shbuf_io; + int ret; + + shbuf_io = remoteproc_get_io_with_pa(rproc, SHM_START_ADDR); + if (!shbuf_io) + return NULL; + shbuf = metal_io_phys_to_virt(shbuf_io, SHM_START_ADDR); + + printf("(2)master creating remoteproc virtio\n"); + vdev = remoteproc_create_virtio(rproc, 0, RPMSG_MASTER, NULL); + if (!vdev) { + printf("failed remoteproc_create_virtio\n"); + return NULL; + } + + printf("(3)master initializing rpmsg shared buffer pool\n"); + /* Only RPMsg virtio master needs to initialize the shared buffers pool */ + rpmsg_virtio_init_shm_pool(&shpool, shbuf, SHM_SIZE); + + printf("(4)master initializing rpmsg vdev\n"); + /* RPMsg virtio slave can set shared buffers pool argument to NULL */ + ret = rpmsg_init_vdev(&rpmsg_vdev, vdev, ns_bind_cb, shbuf_io, &shpool); + if (ret) { + printf("failed rpmsg_init_vdev\n"); + remoteproc_remove_virtio(rproc, vdev); + return NULL; + } + + //printf("master vq[0].callback:%p, vq[1].callback:%p\n", vdev->vrings_info[0].vq->callback, vdev->vrings_info[1].vq->callback); + + printf("(5)master returning rdev\n"); + return rpmsg_virtio_get_rpmsg_device(&rpmsg_vdev); +} + +static void rpmsg_app_master(struct remoteproc *rproc) +{ + int status = 0; + unsigned int message = 2; + + //tmp + /* register notify function */ + rproc->ops->notify = rproc_notify; + + /* 1.2 master receive NS Announcement, and process it */ + receive_message(rproc); + + printf("1.2 master receive_message succeed\n"); + printf("1.2 master Endpoint: name:%s, addr:0x%x, dest_addr:0x%x\n", linux_ept.name, linux_ept.addr, linux_ept.dest_addr); + + while (message < 99) { + /* 2. master send message */ + status = send_message(message, &linux_ept); + if (status < 0) + printf("send_message(%u) failed with status %d\n", message, status); + + printf("2. master send_message succeed\n"); + + sleep(1); + /* 5. master receive message */ + message = receive_message(rproc); + printf("5. Master core received a message: %u\n\n", message); + + message++; + sleep(1); + } +} + +int rpmsg_linux_endpoint() +{ + struct remoteproc *rproc = NULL; + unsigned int id = 1; + struct rpmsg_device *rdev = NULL; + + /* create remoteproc */ + rproc = platform_create_proc(id); + if (!rproc) { + printf("create rproc failed\n"); + return -1; + } + + rdev = platform_create_rpmsg_vdev(rproc); + if (!rdev) + printf("master create rpmsg vdev failed\n"); + + rpmsg_app_master(rproc); + //remoteproc_remove(rproc); + + return 0; +} diff --git a/mcs/openamp_demo_proc_rsctbl/rpmsg_main.c b/mcs/openamp_demo_proc_rsctbl/rpmsg_main.c new file mode 100644 index 0000000000000000000000000000000000000000..611636ae7f6c3854a71b2055c60f9ff036bc137b --- /dev/null +++ b/mcs/openamp_demo_proc_rsctbl/rpmsg_main.c @@ -0,0 +1,76 @@ +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +int pipefd1[2]; +int pipefd2[2]; + +/* Place resource table in special ELF section */ +#define __section_t(S) __attribute__((__section__(#S))) +#define __resource __section_t(.resource_table) + +#define NUM_TABLE_ENTRIES 2 +#define DFEATURE_SUPPORT_NS 1 + +struct remote_resource_table __resource resources = { + /* Version */ + RSC_TAB_SUPPORTED_VERSION, + /* NUmber of table entries */ + NUM_TABLE_ENTRIES, + /* reserved fields */ + { 0, 0 }, + /* Offsets of rsc entries */ + { + offsetof(struct remote_resource_table, rpmsg_mem), + offsetof(struct remote_resource_table, rpmsg_vdev), + }, + + /* 1. Rpmsg buffer memory entry */ + { RSC_CARVEOUT, SHM_START_ADDR, SHM_START_ADDR, SHM_SIZE, 0 }, + + /* 2. Virtio device entry */ + { RSC_VDEV, VIRTIO_ID_RPMSG, VDEV_NOTIFY_ID, DFEATURE_SUPPORT_NS, 0, 0, 0, VRING_COUNT, { 0, 0 } }, + /* Vring rsc entry - part of vdev rsc entry */ + { VRING_RX_ADDRESS, VRING_ALIGNMENT, VRING_SIZE, VRING_RX_NOTIFY_ID, 0 }, /* VRING_RX */ + { VRING_TX_ADDRESS, VRING_ALIGNMENT, VRING_SIZE, VRING_TX_NOTIFY_ID, 0 }, /* VRING_TX */ +}; + +void *get_resource_table(int *len) +{ + *len = sizeof(resources); + return &resources; +} + +extern int rpmsg_linux_endpoint(); +extern int rpmsg_rtos_endpoint(); + +int main(int argc, char **argv) +{ + pid_t pid; + + /* Create unamed pipe */ + if (pipe(pipefd1) < 0) { + perror("pipe1"); + return -1; + } + if (pipe(pipefd2) < 0) { + perror("pipe2"); + return -1; + } + + if ((pid = fork()) < 0) { + perror("fork"); + return -1; + } else if (pid > 0) { + /* parent: Linux Endpoint */ + rpmsg_linux_endpoint(); + } else { + /* child: RTOS Endpoint */ + rpmsg_rtos_endpoint(); + } + + return 0; +} diff --git a/mcs/openamp_demo_proc_rsctbl/rpmsg_rtos_endpoint.c b/mcs/openamp_demo_proc_rsctbl/rpmsg_rtos_endpoint.c new file mode 100644 index 0000000000000000000000000000000000000000..a0ad2872bab320cf59761d2e721d4c9ce41458a5 --- /dev/null +++ b/mcs/openamp_demo_proc_rsctbl/rpmsg_rtos_endpoint.c @@ -0,0 +1,277 @@ +/* RTOS Endpoint run in child process, isolate with parent process */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +static volatile unsigned int rtos_received_data; + +static struct remoteproc *rtos_rproc_init(struct remoteproc *rproc, + struct remoteproc_ops *ops, void *arg) +{ + struct rproc_priv *priv; + unsigned int id = *((unsigned int *)arg); + + priv = metal_allocate_memory(sizeof(*priv)); + if (!priv) + return NULL; + + memset(priv, 0, sizeof(*priv)); + priv->rproc = rproc; + priv->id = id; + priv->rproc->ops = ops; + metal_list_init(&priv->rproc->mems); + priv->rproc->priv = priv; + rproc->state = RPROC_READY; + return priv->rproc; +} + +static void rtos_rproc_remove(struct remoteproc *rproc) +{ + struct rproc_priv *priv; + + priv = (struct rproc_priv *)rproc->priv; + metal_free_memory(priv); +} + +static void *rtos_rproc_mmap(struct remoteproc *rproc, + metal_phys_addr_t *pa, metal_phys_addr_t *da, + size_t size, unsigned int attribute, + struct metal_io_region **io) +{ + struct remoteproc_mem *mem; + struct metal_io_region *tmpio; + + if (*pa == METAL_BAD_PHYS && *da == METAL_BAD_PHYS) + return NULL; + if (*pa == METAL_BAD_PHYS) + *pa = *da; + if (*da == METAL_BAD_PHYS) + *da = *pa; + + mem = metal_allocate_memory(sizeof(*mem)); + if (!mem) + return NULL; + tmpio = metal_allocate_memory(sizeof(*tmpio)); + if (!tmpio) { + metal_free_memory(mem); + return NULL; + } + + /* mmap pa to va */ + int memfd; + void *va; + memfd = open("/dev/mem", O_RDWR); + va = mmap((void*)pa, size, PROT_READ | PROT_WRITE, MAP_SHARED, memfd, *pa); + + remoteproc_init_mem(mem, NULL, *pa, *da, size, tmpio); + metal_io_init(tmpio, va, &mem->pa, size, -1, attribute, NULL); /* metal io manage va and pa */ + remoteproc_add_mem(rproc, mem); + if (io) + *io = tmpio; + + printf("slave mmap pa=0x%lx, da=0x%lx, size=0x%lx, attribute=0x%x va=%p\n", *pa, *da, size, attribute, va); + + return metal_io_phys_to_virt(tmpio, mem->pa); +} + +static int rtos_rproc_notify(struct remoteproc *rproc, uint32_t id) +{ + int ret; + + printf("slave notify start\n"); + + /* notify RTOS Endpoint using IPC */ + ret = write(pipefd2[1], "RTOS: notify ipi\n", strlen("RTOS: notify ipi\n")); + if (ret == -1) + perror("slave write pipefd2[1]"); + + return 0; +} + +static struct remoteproc_ops rtos_rproc_ops = { + .init = rtos_rproc_init, + .remove = rtos_rproc_remove, + .mmap = rtos_rproc_mmap, + .notify = rtos_rproc_notify, +}; + +static unsigned int rtos_receive_message(struct remoteproc *rproc) +{ + char buf[50] = {0}; + int ret; + + /* 1. poll and wait for IPI(IPC) from RTOS Endpoint */ + while (read(pipefd1[0], buf, sizeof(buf)) > 0) { + printf("slave poll:%s", buf); + if (strcmp(buf, "Linux: notify ipi\n") == 0) + break; + } + + /* 2. receive data */ + ret = remoteproc_get_notification(rproc, VRING_TX_NOTIFY_ID); + if (ret) + printf("remoteproc_get_notification failed: 0x%x\n", ret); + return rtos_received_data; +} + +static int rtos_endpoint_cb(struct rpmsg_endpoint *ept, void *data, + size_t len, uint32_t src, void *priv) +{ + rtos_received_data = *((unsigned int *) data); + return RPMSG_SUCCESS; +} + +static void rtos_rpmsg_service_unbind(struct rpmsg_endpoint *ept) +{ + rpmsg_destroy_ept(ept); +} + +static int rtos_send_message(unsigned int message, struct rpmsg_endpoint *ept) +{ + return rpmsg_send(ept, &message, sizeof(message)); +} + +static struct remoteproc *rtos_platform_create_proc(unsigned int id) +{ + struct remoteproc *rproc; + struct remoteproc rproc_inst; + void *va; + void *rsc; + int ret; + metal_phys_addr_t pa = RSC_TABLE_ADDR; + int rsc_size; + + /* Initialize remoteproc instance */ + rproc = remoteproc_init(&rproc_inst, &rtos_rproc_ops, &id); + if (!rproc) + return NULL; + + /* mmap resource table */ + rsc = get_resource_table(&rsc_size); + va = remoteproc_mmap(rproc, &pa, NULL, rsc_size, 0, NULL); + memcpy(va, rsc, rsc_size); + + /* parse resource table to remoteproc */ + ret = remoteproc_set_rsc_table(rproc, va, rsc_size); + if (ret) { + printf("Failed to initialize remoteproc, ret:0x%x\n", ret); + remoteproc_remove(rproc); + return NULL; + } + + //tmp + struct remote_resource_table *va_tmp = va; + struct remote_resource_table *rsc_tmp = rsc; + va_tmp->rpmsg_vdev.notifyid = rsc_tmp->rpmsg_vdev.notifyid; + va_tmp->rpmsg_vdev.vring[0].notifyid = rsc_tmp->rpmsg_vdev.vring[0].notifyid; + va_tmp->rpmsg_vdev.vring[1].notifyid = rsc_tmp->rpmsg_vdev.vring[1].notifyid; + + printf("(1)slave platform_create_proc success\n"); + + return rproc; +} + +static struct rpmsg_device *rtos_platform_create_rpmsg_vdev(struct remoteproc *rproc) +{ + struct rpmsg_virtio_device rpmsg_vdev; + struct virtio_device *vdev; + void *shbuf; + struct metal_io_region *shbuf_io; + int ret; + + shbuf_io = remoteproc_get_io_with_pa(rproc, SHM_START_ADDR); + if (!shbuf_io) + return NULL; + shbuf = metal_io_phys_to_virt(shbuf_io, SHM_START_ADDR); + + printf("(2)slave creating remoteproc virtio\n"); + vdev = remoteproc_create_virtio(rproc, 0, RPMSG_REMOTE, NULL); + if (!vdev) { + printf("failed remoteproc_create_virtio\n"); + return NULL; + } + + printf("(3)slave initializing rpmsg vdev\n"); + /* RPMsg virtio slave can set shared buffers pool argument to NULL */ + ret = rpmsg_init_vdev(&rpmsg_vdev, vdev, NULL, shbuf_io, NULL); + if (ret) { + printf("failed rpmsg_init_vdev\n"); + remoteproc_remove_virtio(rproc, vdev); + return NULL; + } + + //printf("slave vq[0].callback:%p, vq[1].callback:%p\n", vdev->vrings_info[0].vq->callback, vdev->vrings_info[1].vq->callback); + + printf("(4)slave returning rdev\n"); + return rpmsg_virtio_get_rpmsg_device(&rpmsg_vdev); +} + +static void rtos_rpmsg_app_master(struct remoteproc *rproc, struct rpmsg_device *rdev) +{ + int status = 0; + unsigned int message; + struct rpmsg_endpoint ept; + + /* 1.1 slave create endpoint, and send an NS Announcement */ + status = rpmsg_create_ept(&ept, rdev, "k", RPMSG_ADDR_ANY, RPMSG_ADDR_ANY, rtos_endpoint_cb, rtos_rpmsg_service_unbind); + if (status < 0) + printf("slave rpmsg_create_ept failed:%d\n", status); + + printf("1.1 slave rpmsg_create_ept succeed\n"); + printf("1.1 slave Endpoint: name:%s, addr:0x%x, dest_addr:0x%x\n", ept.name, ept.addr, ept.dest_addr); + + while (message < 99) { + /* 3. slave receive message */ + message = rtos_receive_message(rproc); + printf("3. Slave core received a message: %u\n", message); + message++; + sleep(1); + + printf("3. slave Endpoint: name:%s, addr:0x%x, dest_addr:0x%x\n", ept.name, ept.addr, ept.dest_addr); + + /* 4. slave send message */ + status = rtos_send_message(message, &ept); + if (status < 0) + printf("rtos_send_message(%u) failed with status %d\n", message, status); + + printf("4. slave rtos_send_message succeed\n"); + + sleep(1); + } +} + +int rpmsg_rtos_endpoint() +{ + struct remoteproc *rproc = NULL; + unsigned int id = 1; + struct rpmsg_device *rdev = NULL; + + /* create remoteproc */ + rproc = rtos_platform_create_proc(id); + if (!rproc) { + printf("create rproc failed\n"); + return -1; + } + + rdev = rtos_platform_create_rpmsg_vdev(rproc); + if (!rdev) + printf("slave create rpmsg vdev failed\n"); + + rtos_rpmsg_app_master(rproc, rdev); + //remoteproc_remove(rproc); + + return 0; +} diff --git a/mcs/openamp_demo_pty_multithread/Makefile b/mcs/openamp_demo_pty_multithread/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..2e7a8f5c4029dce6c81e571d4faf8618759ab907 --- /dev/null +++ b/mcs/openamp_demo_pty_multithread/Makefile @@ -0,0 +1,14 @@ +rpmsg_main: rpmsg_main.o rpmsg_ping.o rpmsg_pty.o + $(CC) -g rpmsg_main.o rpmsg_ping.o rpmsg_pty.o $(SDKTARGETSYSROOT)/usr/lib64/libmetal.so $(SDKTARGETSYSROOT)/usr/lib64/libopen_amp.so $(SDKTARGETSYSROOT)/lib64/libsysfs.so $(SDKTARGETSYSROOT)/lib64/libpthread.so.0 -o rpmsg_main + +rpmsg_main.o: rpmsg_main.c rpmsg-internal.h + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_main.c -o rpmsg_main.o + +rpmsg_ping.o: rpmsg_ping.c rpmsg-internal.h + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_ping.c -o rpmsg_ping.o + +rpmsg_pty.o: rpmsg_pty.c + $(CC) -g -I$(SDKTARGETSYSROOT)/usr/include -c rpmsg_pty.c -o rpmsg_pty.o + +clean: + rm -rf *.o rpmsg_main diff --git a/mcs/openamp_demo_pty_multithread/rpmsg-internal.h b/mcs/openamp_demo_pty_multithread/rpmsg-internal.h new file mode 100644 index 0000000000000000000000000000000000000000..105d6c7c7ab800ba81e27c8b0591a7078a879999 --- /dev/null +++ b/mcs/openamp_demo_pty_multithread/rpmsg-internal.h @@ -0,0 +1,43 @@ +#ifndef RPMSG_INTERNAL_H_ +#define RPMSG_INTERNAL_H_ + +#include +#include +#include + +#define VDEV_START_ADDR 0x70000000 +#define VDEV_SIZE 0x30000 + +#define VDEV_STATUS_ADDR VDEV_START_ADDR +#define VDEV_STATUS_SIZE 0x4000 + +#define SHM_START_ADDR (VDEV_START_ADDR + VDEV_STATUS_SIZE) +#define SHM_SIZE (VDEV_SIZE - VDEV_STATUS_SIZE) + +#define VRING_COUNT 2 +#define VRING_RX_ADDRESS (VDEV_START_ADDR + SHM_SIZE - VDEV_STATUS_SIZE) +#define VRING_TX_ADDRESS (VDEV_START_ADDR + SHM_SIZE) +#define VRING_ALIGNMENT 4 +#define VRING_SIZE 16 + +#define IRQ_SENDTO_CLIENTOS _IOW('A', 0, int) +#define DEV_CLIENT_OS_AGENT "/dev/cpu_handler" + +extern char *cpu_id; +extern struct metal_io_region *io; + +struct thread_args { + int fd; + int *pipefd; +}; + +struct remoteproc *platform_create_proc(unsigned int id); +int load_bin(void); +void rpmsg_endpoint_init(void); +int rpmsg_endpoint_app(int fds, int ns_setup); + +void open_pty(int *pfdm, int *pfds); +void *master(void *arg_list); +void *slave(void *arg_list); + +#endif diff --git a/mcs/openamp_demo_pty_multithread/rpmsg_main.c b/mcs/openamp_demo_pty_multithread/rpmsg_main.c new file mode 100644 index 0000000000000000000000000000000000000000..b8e230baa1a03731d3e25f5c6076178b073bc669 --- /dev/null +++ b/mcs/openamp_demo_pty_multithread/rpmsg_main.c @@ -0,0 +1,216 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +#define MAX_BIN_BUFLEN (10 * 1024 * 1024) +#define BOOTCMD_MAXSIZE 100 + +static struct remoteproc rproc_inst; +static struct remoteproc_ops ops; + +char *cpu_id; +static char *boot_address; +static char *target_binfile; +static char *target_binaddr; + +struct rproc_priv { + struct remoteproc *rproc; + unsigned int id; +}; + +static void cleanup(void) +{ + printf("\nOpenAMP demo ended.\n"); + remoteproc_remove(&rproc_inst); + if (io) + free(io); +} + +static void handler(int sig) +{ + exit(0); +} + +int load_bin(void) +{ + int memfd = open("/dev/mem", O_RDWR); + int bin_fd = open(target_binfile, O_RDONLY); + void *access_address = NULL, *bin_buffer = NULL; + long bin_size; + long long int bin_addr = strtoll(target_binaddr, NULL, 0); + + if (bin_fd < 0 || memfd < 0) { + printf("invalid bin file fd\n"); + exit(-1); + } + + bin_buffer = (void *)malloc(MAX_BIN_BUFLEN); + if (!bin_buffer) { + printf("malloc bin_buffer failed\n"); + exit(-1); + } + + bin_size = read(bin_fd, bin_buffer, MAX_BIN_BUFLEN); + if (bin_size == 0) { + printf("read bin file failed\n"); + exit(-1); + } + + access_address = mmap((void *)bin_addr, MAX_BIN_BUFLEN, PROT_READ | PROT_WRITE, + MAP_SHARED, memfd, bin_addr); + memcpy(access_address, bin_buffer, bin_size); + free(bin_buffer); + return 0; +} + +static struct remoteproc *rproc_init(struct remoteproc *rproc, + struct remoteproc_ops *ops, void *arg) +{ + struct rproc_priv *priv; + unsigned int id = *((unsigned int *)arg); + + priv = metal_allocate_memory(sizeof(*priv)); + if (!priv) + return NULL; + + memset(priv, 0, sizeof(*priv)); + priv->rproc = rproc; + priv->id = id; + priv->rproc->ops = ops; + metal_list_init(&priv->rproc->mems); + priv->rproc->priv = priv; + rproc->state = RPROC_READY; + return priv->rproc; +} + +static int rproc_start(struct remoteproc *rproc) +{ + int cpu_handler_fd; + int ret; + char on[BOOTCMD_MAXSIZE]; + + (void)snprintf(on, sizeof(on), "%s%s%s", cpu_id, "@", boot_address); + + cpu_handler_fd = open(DEV_CLIENT_OS_AGENT, O_RDWR); + if (cpu_handler_fd < 0) { + printf("failed to open %s\n", DEV_CLIENT_OS_AGENT); + return cpu_handler_fd; + } + + ret = write(cpu_handler_fd, on, sizeof(on)); + return 0; +} + +static void rproc_remove(struct remoteproc *rproc) +{ + struct rproc_priv *priv; + + priv = (struct rproc_priv *)rproc->priv; + metal_free_memory(priv); +} + +struct remoteproc_ops rproc_ops = { + .init = rproc_init, + .remove = rproc_remove, + .start = rproc_start, +}; + +struct remoteproc *platform_create_proc(unsigned int id) +{ + struct remoteproc *rproc; + + ops = rproc_ops; + rproc = remoteproc_init(&rproc_inst, &ops, &id); + if (!rproc) + return NULL; + + return &rproc_inst; +} + +int main(int argc, char **argv) +{ + int opt; + int fdm = -1, fds = -1; + pid_t pid; + int pipefd[2]; + pthread_t tidm, tids; + struct thread_args args; + void *retval; +#if 0 + while ((opt = getopt(argc, argv, "c:b:t:a:")) != -1) { + switch (opt) { + case 'c': + cpu_id = optarg; + break; + case 'b': + boot_address = optarg; + break; + case 't': + target_binfile = optarg; + break; + case 'a': + target_binaddr = optarg; + break; + default: + break; + } + } +#endif + + cpu_id = "3"; + boot_address = "0x7a000ffc"; + target_binfile = "zephyr.bin"; + target_binaddr = "0x7a000000"; + + /* Open the master side and the slave side of the PTY */ + open_pty(&fdm, &fds); + + /* Create unamed pipe */ + if (pipe(pipefd) < 0) { + perror("pipe"); + return -1; + } + + /* Create the child process */ + if ((pid = fork()) < 0) { + perror("fork"); + return -1; + } else if (pid > 0) { + close(fds); /* Close the slave side of the PTY */ + + args.fd = fdm; + args.pipefd = pipefd; + if (pthread_create(&tidm, NULL, master, (void*)&args) < 0) { //T3 + perror("parent pthread_create"); + exit(1); + } + //printf("This is thread1: %ld, pid:%d\n", pthread_self(), getpid()); //T1 + pthread_join(tidm, NULL); + } else { + /* ctrl+c signal, exit program and do cleanup */ + atexit(cleanup); + signal(SIGINT, handler); + + close(fdm); /* Close the master side of the PTY */ + + args.fd = fds; + args.pipefd = pipefd; + if (pthread_create(&tids, NULL, slave, (void*)&args) < 0) { //T4 + perror("child pthread_create"); + exit(1); + } + //printf("This is thread2: %ld, pid:%d\n", pthread_self(), getpid()); //T2 + pthread_join(tids, &retval); + if (*(int*)retval) + exit(1); + } + + return 0; +} diff --git a/mcs/openamp_demo_pty_multithread/rpmsg_ping.c b/mcs/openamp_demo_pty_multithread/rpmsg_ping.c new file mode 100644 index 0000000000000000000000000000000000000000..722b0853c79d399dddf5a9ca2ffdb0dd2b1ff95a --- /dev/null +++ b/mcs/openamp_demo_pty_multithread/rpmsg_ping.c @@ -0,0 +1,220 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "rpmsg-internal.h" + +static struct virtio_vring_info rvrings[2] = { + [0] = { + .info.align = VRING_ALIGNMENT, + }, + [1] = { + .info.align = VRING_ALIGNMENT, + }, +}; + +static int g_memfd; +volatile unsigned int received_data; +static struct virtio_device vdev; +static struct rpmsg_virtio_device rvdev; +struct metal_io_region *io; +struct virtqueue *vq[2]; +static void *tx_addr, *rx_addr, *shm_start_addr; +static metal_phys_addr_t shm_physmap[] = { SHM_START_ADDR }; + +static unsigned char virtio_get_status(struct virtio_device *vdev) +{ + return VIRTIO_CONFIG_STATUS_DRIVER_OK; +} + +static void virtio_set_status(struct virtio_device *vdev, unsigned char status) +{ + void *stat_addr = NULL; + stat_addr = mmap((void *)VDEV_STATUS_ADDR, VDEV_STATUS_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, g_memfd, VDEV_STATUS_ADDR); + *(volatile unsigned int *)stat_addr = (unsigned int)status; +} + +static uint32_t virtio_get_features(struct virtio_device *vdev) +{ + return 1 << VIRTIO_RPMSG_F_NS; +} + +static void virtio_notify(struct virtqueue *vq) +{ + (void)vq; + int cpu_handler_fd; + int ret; + int cpu_num = strtol(cpu_id, NULL, 0); + + cpu_handler_fd = open(DEV_CLIENT_OS_AGENT, O_RDWR); + if (cpu_handler_fd < 0) { + printf("open %s failed\n", DEV_CLIENT_OS_AGENT); + return; + } + + ret = ioctl(cpu_handler_fd, IRQ_SENDTO_CLIENTOS, cpu_num); + if (ret) { + printf("send ipi tp second os failed\n"); + } + + close(cpu_handler_fd); + return; +} + +struct virtio_dispatch dispatch = { + .get_status = virtio_get_status, + .set_status = virtio_set_status, + .get_features = virtio_get_features, + .notify = virtio_notify, +}; + +int endpoint_cb(struct rpmsg_endpoint *ept, void *data, + size_t len, uint32_t src, void *priv) +{ + received_data = *((unsigned int *) data); + return RPMSG_SUCCESS; +} + +struct rpmsg_endpoint my_ept; +struct rpmsg_endpoint *ep = &my_ept; + +static void rpmsg_service_unbind(struct rpmsg_endpoint *ept) +{ + (void)ept; + rpmsg_destroy_ept(ep); +} + +void ns_bind_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest) +{ + (void)rpmsg_create_ept(ep, rdev, name, + RPMSG_ADDR_ANY, dest, + endpoint_cb, + rpmsg_service_unbind); +} + +static struct rpmsg_virtio_shm_pool shpool; + +void rpmsg_endpoint_init(void) +{ + int status = 0; + + g_memfd = open("/dev/mem", O_RDWR); + tx_addr = mmap((void *)VRING_TX_ADDRESS, VDEV_STATUS_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, g_memfd, VRING_TX_ADDRESS); + rx_addr = mmap((void *)VRING_RX_ADDRESS, VDEV_STATUS_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, g_memfd, VRING_RX_ADDRESS); + shm_start_addr = mmap((void *)SHM_START_ADDR, SHM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, g_memfd, SHM_START_ADDR); + + io = malloc(sizeof(struct metal_io_region)); + if (!io) { + printf("malloc io failed\n"); + return; + } + metal_io_init(io, shm_start_addr, shm_physmap, SHM_SIZE, -1, 0, NULL); + + /* setup vdev */ + vq[0] = virtqueue_allocate(VRING_SIZE); + if (vq[0] == NULL) { + printf("virtqueue_allocate failed to alloc vq[0]\n"); + free(io); + return; + } + vq[1] = virtqueue_allocate(VRING_SIZE); + if (vq[1] == NULL) { + printf("virtqueue_allocate failed to alloc vq[1]\n"); + free(io); + return; + } + + vdev.role = RPMSG_MASTER; + vdev.vrings_num = VRING_COUNT; + vdev.func = &dispatch; + rvrings[0].io = io; + rvrings[0].info.vaddr = tx_addr; + rvrings[0].info.num_descs = VRING_SIZE; + rvrings[0].info.align = VRING_ALIGNMENT; + rvrings[0].vq = vq[0]; + + rvrings[1].io = io; + rvrings[1].info.vaddr = rx_addr; + rvrings[1].info.num_descs = VRING_SIZE; + rvrings[1].info.align = VRING_ALIGNMENT; + rvrings[1].vq = vq[1]; + + vdev.vrings_info = &rvrings[0]; + + /* setup rvdev */ + rpmsg_virtio_init_shm_pool(&shpool, shm_start_addr, SHM_SIZE); + status = rpmsg_init_vdev(&rvdev, &vdev, ns_bind_cb, io, &shpool); + if (status != 0) { + printf("rpmsg_init_vdev failed %d\n", status); + free(io); + return; + } +} + +int rpmsg_endpoint_app(int fds, int ns_setup) +{ + int cpu_handler_fd; + struct pollfd poll_fd[2] = {0}; + + cpu_handler_fd = open(DEV_CLIENT_OS_AGENT, O_RDWR); + if (cpu_handler_fd < 0) { + printf("open %s failed.\n", DEV_CLIENT_OS_AGENT); + return -1; + } + + poll_fd[0].fd = fds; + poll_fd[0].events = POLLIN; + poll_fd[1].fd = cpu_handler_fd; + poll_fd[1].events = POLLIN; + + /* Wait for data from slave side of PTY, and data from endpoint of openamp, blocked */ + if (poll(poll_fd, 2, -1) == -1) { + printf("Error %d on poll()\n", errno); + goto err; + } + + close(cpu_handler_fd); + + /* If data on slave side of PTY */ + if (poll_fd[0].revents & POLLIN) { + char recv_buf[200] = {0}; + unsigned int number; + + poll_fd[0].revents = 0; + if (read(fds, recv_buf, sizeof(recv_buf)) > 0) { + number = atoi(recv_buf); + //printf("Master core sending messages: %d\n", number); + rpmsg_send(ep, &number, sizeof(number)); + } + } + + /* If data on endpoint of openamp */ + if (poll_fd[1].revents & POLLIN) { + char send_buf[200] = {0}; + int wc; + + poll_fd[1].revents = 0; + virtqueue_notification(vq[0]); + //printf("Master core receiving messages: %d\n", received_data); + + /* Check whether poll event is ns setup */ + if (ns_setup == 1) + return 0; + + received_data++; + wc = snprintf(send_buf, sizeof(send_buf), "Output number: %d\n", received_data); + if (write(fds, send_buf, wc) == -1) { + printf("Error %d on rpmsg_endpoint_app write()\n", errno); + goto err; + } + } + + return 0; +err: + close(cpu_handler_fd); + return -1; +} diff --git a/mcs/openamp_demo_pty_multithread/rpmsg_pty.c b/mcs/openamp_demo_pty_multithread/rpmsg_pty.c new file mode 100644 index 0000000000000000000000000000000000000000..57251928fb819e827bcc77d953f3146c637bdd62 --- /dev/null +++ b/mcs/openamp_demo_pty_multithread/rpmsg_pty.c @@ -0,0 +1,153 @@ +#define _XOPEN_SOURCE 600 +#define _DEFAULT_SOURCE +#include +#include +#include +#include +#include +#include + +#include + +#include "rpmsg-internal.h" + +void open_pty(int *pfdm, int *pfds) +{ + int rc, fdm, fds; + + /* Open the master side of the PTY */ + fdm = posix_openpt(O_RDWR | O_NOCTTY); + if (fdm < 0) + printf("Error %d on posix_openpt()\n", errno); + + rc = grantpt(fdm); + if (rc != 0) + printf("Error %d on grantpt()\n", errno); + + rc = unlockpt(fdm); + if (rc != 0) + printf("Error %d on unlockpt()\n", errno); + + /* Open the slave side of the PTY */ + fds = open(ptsname(fdm), O_RDWR | O_NOCTTY); + + *pfdm = fdm; + *pfds = fds; +} + +static void ptmx_read_write(const char *name, int in, int out) +{ + char input[200]; + int rc, ret; + + rc = read(in, input, sizeof(input)); + if (rc == -1) + printf("Error %d on ptmx_read %s\n", errno, name); + + ret = write(out, input, rc); + if (ret == -1) + printf("Error %d on ptmx_write %s\n", errno, name); +} + +void *master(void *arg_list) +{ + fd_set fd_in; + char buf[50] = {0}; + int nullfd; + struct thread_args *args = (struct thread_args*)arg_list; + + /* Wait for child */ + close(args->pipefd[1]); + while (read(args->pipefd[0], buf, sizeof(buf)) > 0) { + if (strcmp(buf, "init done\n") == 0) + break; + } + + //printf("This is thread3: %ld, pid:%d\n", pthread_self(), getpid()); //T3 + //printf("thread3 fd:%d, pipefd[0]:%d, pipefd[1]:%d\n", args->fd, args->pipefd[0], args->pipefd[1]); + + printf("----------PTY Terminal----------\n"); + printf("Input number: "); + fflush(stdout); + + for (;;) { + /* Wait for data from standard input and master side of PTY */ + FD_ZERO(&fd_in); + FD_SET(0, &fd_in); + FD_SET(args->fd, &fd_in); + + if (select(args->fd + 1, &fd_in, NULL, NULL, NULL) == -1) + printf("Error %d on select()\n", errno); + + /* If data on standard input */ + if (FD_ISSET(0, &fd_in)) { + ptmx_read_write("standard input", 0, args->fd); + + /* flush data to /dev/null on ptmx, avoid printing repeatedly */ + nullfd = open("/dev/null", O_RDWR); + ptmx_read_write("standard input", args->fd, nullfd); + } + + /* If data on master side of PTY */ + if (FD_ISSET(args->fd, &fd_in)) { + ptmx_read_write("master pty", args->fd, 1); + printf("Input number: "); + fflush(stdout); + } + } +} + +void *slave(void *arg_list) +{ + struct remoteproc *rproc = NULL; + unsigned int id = 1; + int ret; + struct thread_args *args = (struct thread_args*)arg_list; + + //printf("This is thread4: %ld, pid:%d\n", pthread_self(), getpid()); //T4 + //printf("thread4 fd:%d, pipefd[0]:%d, pipefd[1]:%d\n", args->fd, args->pipefd[0], args->pipefd[1]); + + rproc = platform_create_proc(id); + if (!rproc) { + printf("create rproc failed\n"); + return (void*)1; + } + + ret = load_bin(); + if (ret) { + printf("failed to load client os\n"); + return (void*)1; + } + + ret = remoteproc_start(rproc); + if (ret) { + printf("start processor failed\n"); + return (void*)1; + } + + sleep(5); /* wait for clientos booting */ + rpmsg_endpoint_init(); + + /* Since we are using name service, we need to wait for a response + * from NS setup and then we need to process it + */ + rpmsg_endpoint_app(args->fd, 1); + + /* now wake up parent using unamed pipe, init console */ + close(args->pipefd[0]); + ret = write(args->pipefd[1], "init done\n", strlen("init done\n")); + if (ret == -1) { + printf("failed to wake up parent\n"); + return (void*)1; + } + + for (;;) { + ret = rpmsg_endpoint_app(args->fd, 0); + if (ret) { + printf("rpmsg_endpoint_app failed\n"); + return (void*)1; + } + } + + return (void*)0; +}