diff --git a/mcs/latency_demo/rpmsg_main.c b/mcs/latency_demo/rpmsg_main.c index 44c3ad81a134a1324fc9d06cc705c8abe0eee6d4..2632ee0d8c52f1ca44bde5e10847478cbb677eee 100644 --- a/mcs/latency_demo/rpmsg_main.c +++ b/mcs/latency_demo/rpmsg_main.c @@ -9,6 +9,7 @@ char *target_binaddr; static void cleanup(int sig) { openamp_deinit(); + exit(0); } int rpmsg_app_master(void) diff --git a/mcs/latency_demo/zephyr_rpi.bin b/mcs/latency_demo/zephyr_rpi.bin index d665b60013137677d86358ebb1c8abfa1babbbf4..2f3b03354f2e30aa6cb0e72b828b9866ac6d5cbd 100644 Binary files a/mcs/latency_demo/zephyr_rpi.bin and b/mcs/latency_demo/zephyr_rpi.bin differ diff --git a/mcs/mcs_km/mcs_km.c b/mcs/mcs_km/mcs_km.c index de1dd1607a1aa7e675a42b7ed2c797c62096d965..f7a32af437edd752b05822c681d861adef07fef2 100644 --- a/mcs/mcs_km/mcs_km.c +++ b/mcs/mcs_km/mcs_km.c @@ -14,10 +14,12 @@ #define MCS_DEVICE_NAME "mcs" #define CPU_ON_FUNCID 0xC4000003 +#define AFFINITY_INFO_FUNCID 0xC4000004 #define MAGIC_NUMBER 'A' #define IOC_SENDIPI _IOW(MAGIC_NUMBER, 0, int) #define IOC_CPUON _IOW(MAGIC_NUMBER, 1, int) +#define IOC_AFFINITY_INFO _IOW(MAGIC_NUMBER, 2, int) #define IOC_MAXNR 2 #define OPENAMP_INIT_TRIGGER 0x0 @@ -29,11 +31,10 @@ static struct class *mcs_class; static int mcs_major; -static int cpu_state; static u64 valid_start; static u64 valid_end; -static const char *smccc_method; +static const char *smccc_method = "hvc"; static DECLARE_WAIT_QUEUE_HEAD(openamp_trigger_wait); static int openamp_trigger; @@ -68,15 +69,6 @@ static void send_clientos_ipi(const struct cpumask *target) ipi_send_mask(OPENAMP_IRQ, target); } -static ssize_t mcs_read(struct file *file, char __user *buf, size_t count, loff_t *ppos) -{ - ssize_t len; - char res[32]; - - len = scnprintf(res, sizeof(res), "%d\n", cpu_state); - return simple_read_from_buffer(buf, count, ppos, res, len); -} - static unsigned int mcs_poll(struct file *file, poll_table *wait) { unsigned int mask; @@ -102,38 +94,43 @@ static long mcs_ioctl(struct file *f, unsigned int cmd, unsigned long arg) return -EINVAL; err = !access_ok((void*)arg, _IOC_SIZE(cmd)); if (err) - return -EINVAL; + return -EINVAL; switch (cmd) { case IOC_SENDIPI: cpu_id = (int)arg; pr_info("mcs_km: received ioctl cmd to send ipi to cpu(%d)\n", cpu_id); send_clientos_ipi(cpumask_of(cpu_id)); - return 0; + break; case IOC_CPUON: cpu_id = *(unsigned int*)arg; cpu_boot_addr = *((unsigned int*)arg + 1); - if (cpu_state == 1) { - pr_info("mcs_km: clientos already boot on cpu(%d)\n", cpu_id); - return 0; + pr_info("mcs_km: start booting clientos on cpu(%d) addr(0x%x) smccc(%s)\n", cpu_id, cpu_boot_addr, smccc_method); + if (strcmp(smccc_method, "smc") == 0) + arm_smccc_smc(CPU_ON_FUNCID, cpu_id, cpu_boot_addr, 0, 0, 0, 0, 0, &res); + else + arm_smccc_hvc(CPU_ON_FUNCID, cpu_id, cpu_boot_addr, 0, 0, 0, 0, 0, &res); + if (res.a0) { + pr_err("mcs_km: boot clientos failed(%ld)\n", res.a0); + return -EINVAL; } + break; + case IOC_AFFINITY_INFO: + cpu_id = *(unsigned int*)arg; - if (smccc_method == NULL) - smccc_method = "hvc"; - - if (cpu_state == 0) { - pr_info("mcs_km: start booting clientos on cpu(%d) addr(0x%x) smccc(%s)\n", cpu_id, cpu_boot_addr, smccc_method); - if (strcmp(smccc_method, "smc") == 0) - arm_smccc_smc(CPU_ON_FUNCID, cpu_id, cpu_boot_addr, 0, 0, 0, 0, 0, &res); - else - arm_smccc_hvc(CPU_ON_FUNCID, cpu_id, cpu_boot_addr, 0, 0, 0, 0, 0, &res); - cpu_state = 1; - } - return 0; + if (strcmp(smccc_method, "smc") == 0) + arm_smccc_smc(AFFINITY_INFO_FUNCID, cpu_id, 0, 0, 0, 0, 0, 0, &res); + else + arm_smccc_hvc(AFFINITY_INFO_FUNCID, cpu_id, 0, 0, 0, 0, 0, 0, &res); + *(unsigned int*)arg = res.a0; + break; default: + pr_err("mcs_km: IOC param invalid(0x%x)\n", cmd); return -EINVAL; } + + return 0; } #ifdef CONFIG_STRICT_DEVMEM @@ -235,7 +232,6 @@ static int mcs_open(struct inode *inode, struct file *filp) static const struct file_operations mcs_fops = { .open = mcs_open, .mmap = mcs_mmap, - .read = mcs_read, .poll = mcs_poll, .unlocked_ioctl = mcs_ioctl, .llseek = generic_file_llseek, diff --git a/mcs/modules/openamp_module.c b/mcs/modules/openamp_module.c index be788c19aa9f8f28bbb15be53a9417d00bec94ca..fbe385c3da50f7b5c3ca59d47599214bb6d9ba74 100644 --- a/mcs/modules/openamp_module.c +++ b/mcs/modules/openamp_module.c @@ -51,15 +51,35 @@ static int reserved_mem_init(void) file_addr = mmap(NULL, binsize, PROT_READ, MAP_PRIVATE, binfd, 0); memcpy(binaddr, file_addr, binsize); + close(binfd); + return 0; } +static void reserved_mem_release(void) +{ + if (shmaddr) + munmap(shmaddr, VDEV_SIZE); + if (binaddr) + munmap(binaddr, binsize); + if (memfd) + close(memfd); +} + int openamp_init(void) { int ret; struct remoteproc *rproc; unsigned char message[100]; int len; + int cpu_state; + + /* secondary core power state must be CPU_STATE_OFF, avoid initialize repeatedly */ + cpu_state = acquire_cpu_state(); + if (cpu_state != CPU_STATE_OFF) { + printf("cpu(%s) is already on(%d).\n", cpu_id, cpu_state); + return -1; + } ret = reserved_mem_init(); if (ret) { @@ -91,13 +111,7 @@ void openamp_deinit(void) { printf("\nOpenAMP demo ended.\n"); + destory_remoteproc(); /* shutdown clientos first */ virtio_deinit(); - destory_remoteproc(); - - if (shmaddr) - munmap(shmaddr, VDEV_SIZE); - if (binaddr) - munmap(binaddr, binsize); - if (memfd) - close(memfd); + reserved_mem_release(); } diff --git a/mcs/modules/openamp_module.h b/mcs/modules/openamp_module.h index 90261620e30788c071b34fdffa99f3cf59409703..bc4237aba810248cbd20bea3e91d7bb784fad858 100644 --- a/mcs/modules/openamp_module.h +++ b/mcs/modules/openamp_module.h @@ -8,6 +8,7 @@ extern char *target_binfile; extern char *target_binaddr; +extern char *cpu_id; /* initialize openamp module, including remoteproc, virtio, rpmsg */ int openamp_init(void); diff --git a/mcs/modules/remoteproc_module.c b/mcs/modules/remoteproc_module.c index 7cb1f65e5d27bcba92e0a15cbcc321d18aba9d7a..2896ded1b45e8eacdd5993548f6feb72ed4818b9 100644 --- a/mcs/modules/remoteproc_module.c +++ b/mcs/modules/remoteproc_module.c @@ -3,9 +3,11 @@ #include #include #include "remoteproc_module.h" +#include "openamp_module.h" #define MCS_DEVICE_NAME "/dev/mcs" #define IOC_CPUON _IOW('A', 1, int) +#define IOC_AFFINITY_INFO _IOW('A', 2, int) #define STR_TO_HEX 16 #define STR_TO_DEC 10 @@ -71,6 +73,8 @@ static int rproc_start(struct remoteproc *rproc) static int rproc_stop(struct remoteproc *rproc) { /* TODO: send message to clientos, clientos shut itself down by PSCI */ + send_message("shutdown", sizeof("shutdown")); + return 0; } @@ -104,3 +108,26 @@ void destory_remoteproc(void) if (rproc_inst.priv) remoteproc_remove(&rproc_inst); } + +int acquire_cpu_state(void) +{ + int ret; + int fd; + unsigned int state_arg; + + fd = open(MCS_DEVICE_NAME, O_RDWR); + if (fd < 0) { + printf("open %s device failed\n", MCS_DEVICE_NAME); + return fd; + } + + state_arg = strtol(cpu_id, NULL, STR_TO_DEC); + ret = ioctl(fd, IOC_AFFINITY_INFO, &state_arg); + if (ret) { + printf("acquire cpu state failed\n"); + return ret; + } + + close(fd); + return state_arg; /* secondary core power state */ +} diff --git a/mcs/modules/remoteproc_module.h b/mcs/modules/remoteproc_module.h index 7b1378bad81d0cdfeffc72657577bb3e274d084b..6b0a6ae2f36cf23f398d194b116c488bcf102caf 100644 --- a/mcs/modules/remoteproc_module.h +++ b/mcs/modules/remoteproc_module.h @@ -3,6 +3,10 @@ #include +#define CPU_STATE_ON 0 +#define CPU_STATE_OFF 1 +#define CPU_STATE_ON_PENDING 2 + /* create remoteproc */ struct remoteproc *create_remoteproc(void); @@ -24,6 +28,9 @@ struct remoteproc *create_remoteproc(void); /* destory remoteproc */ void destory_remoteproc(void); +/* acquire cpu power state */ +int acquire_cpu_state(void); + extern char *cpu_id; extern char *target_binaddr; diff --git a/mcs/modules/virtio_module.c b/mcs/modules/virtio_module.c index 892bd04689adb737b8e08cb46264b6f02b19419c..624822cfa0c6d3ecc04887252a8ce07c310b61c6 100644 --- a/mcs/modules/virtio_module.c +++ b/mcs/modules/virtio_module.c @@ -125,11 +125,7 @@ void virtio_init(void) void virtio_deinit(void) { + rpmsg_deinit_vdev(&rvdev); if (io) free(io); - if (vq[0]) - virtqueue_free(vq[0]); - if (vq[1]) - virtqueue_free(vq[1]); - rpmsg_deinit_vdev(&rvdev); } diff --git a/mcs/openamp_demo/rpmsg_main.c b/mcs/openamp_demo/rpmsg_main.c index 2087650ddf825ae33b98d80541bfe38becd03bf7..91e5e2e1b7e5606a0c2c3d178e0912ec61194367 100644 --- a/mcs/openamp_demo/rpmsg_main.c +++ b/mcs/openamp_demo/rpmsg_main.c @@ -9,6 +9,7 @@ char *target_binaddr; static void cleanup(int sig) { openamp_deinit(); + exit(0); } int rpmsg_app_master(void) diff --git a/mcs/openamp_demo/zephyr_qemu.bin b/mcs/openamp_demo/zephyr_qemu.bin index 585dd2d37f30ff057677080ce98ed06fb1b0c2ae..457b597f7230ab5f7ebec7ca1e0d2d2dec25fc30 100644 Binary files a/mcs/openamp_demo/zephyr_qemu.bin and b/mcs/openamp_demo/zephyr_qemu.bin differ diff --git a/mcs/openamp_demo/zephyr_rpi.bin b/mcs/openamp_demo/zephyr_rpi.bin index 68f8fda8483ece2f6fcd8c8846740fd745679858..5dcb9d9d185945fbb78cecaac70cfb103dc3f445 100644 Binary files a/mcs/openamp_demo/zephyr_rpi.bin and b/mcs/openamp_demo/zephyr_rpi.bin differ diff --git a/mcs/rpmsg_pty_demo/rpmsg_main.c b/mcs/rpmsg_pty_demo/rpmsg_main.c index abd1a7c160349410120cfd733caf37fca0c9b9ff..74fa2f493baeb79407c370712b2006a09727d4cb 100644 --- a/mcs/rpmsg_pty_demo/rpmsg_main.c +++ b/mcs/rpmsg_pty_demo/rpmsg_main.c @@ -12,6 +12,7 @@ static void cleanup(int sig) { openamp_deinit(); pthread_mutex_destroy(&mutex); + exit(0); } int rpmsg_app_master(void) diff --git a/mcs/rpmsg_pty_demo/zephyr_qemu.bin b/mcs/rpmsg_pty_demo/zephyr_qemu.bin index 6d566c0d5ca6202e6b8710761ae16aea691b8fca..ebf342c54a341d7e2cec636e321e07725ea3c368 100644 Binary files a/mcs/rpmsg_pty_demo/zephyr_qemu.bin and b/mcs/rpmsg_pty_demo/zephyr_qemu.bin differ diff --git a/mcs/rpmsg_pty_demo/zephyr_rpi.bin b/mcs/rpmsg_pty_demo/zephyr_rpi.bin index af94a220f6e547f4b27b068c3436856c4c19f03b..57d2c401ce021aa9230ca319000d312c1c829603 100644 Binary files a/mcs/rpmsg_pty_demo/zephyr_rpi.bin and b/mcs/rpmsg_pty_demo/zephyr_rpi.bin differ