2016-02-10 21:58:21 +01:00
|
|
|
/****************************************************************************
|
|
|
|
* arch/sim/src/up_simsmp.c
|
|
|
|
*
|
|
|
|
* Copyright (C) 2016 Gregory Nutt. All rights reserved.
|
|
|
|
* Author: Gregory Nutt <gnutt@nuttx.org>
|
|
|
|
*
|
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
* modification, are permitted provided that the following conditions
|
|
|
|
* are met:
|
|
|
|
*
|
|
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer.
|
|
|
|
* 2. 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.
|
|
|
|
* 3. Neither the name NuttX 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.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Included Files
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-18 18:46:08 +01:00
|
|
|
#define _GNU_SOURCE 1
|
|
|
|
|
2016-02-16 17:12:06 +01:00
|
|
|
#include <stdint.h>
|
2016-02-10 21:58:21 +01:00
|
|
|
#include <pthread.h>
|
|
|
|
#include <semaphore.h>
|
2016-02-18 18:12:43 +01:00
|
|
|
#include <signal.h>
|
|
|
|
#include <errno.h>
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Pre-processor Definitions
|
|
|
|
****************************************************************************/
|
|
|
|
/* Must match definitions in arch/sim/include/spinlock.h */
|
|
|
|
|
|
|
|
#define SP_UNLOCKED 0 /* The Un-locked state */
|
|
|
|
#define SP_LOCKED 1 /* The Locked state */
|
2016-02-10 21:58:21 +01:00
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Types
|
|
|
|
****************************************************************************/
|
2016-02-18 18:12:43 +01:00
|
|
|
/* Must match definitions in arch/sim/include/spinlock.h. Assuming that
|
|
|
|
* bool and unsigned char are equivalent.
|
|
|
|
*/
|
|
|
|
|
|
|
|
typedef unsigned char spinlock_t;
|
|
|
|
|
|
|
|
/* Task entry point type */
|
2016-02-10 21:58:21 +01:00
|
|
|
|
|
|
|
typedef int (*main_t)(int argc, char **argv);
|
|
|
|
|
|
|
|
struct sim_cpuinfo_s
|
|
|
|
{
|
|
|
|
int cpu; /* CPU number */
|
|
|
|
pthread_mutex_t mutex; /* For synchronization */
|
|
|
|
main_t idletask; /* IDLE task entry point */
|
|
|
|
};
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Data
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-18 18:12:43 +01:00
|
|
|
static pthread_key_t g_cpukey;
|
|
|
|
static pthread_t g_sim_cputhread[CONFIG_SMP_NCPUS];
|
|
|
|
static volatile unsigned char g_sim_cpupaused[CONFIG_SMP_NCPUS];
|
|
|
|
static volatile spinlock_t g_sim_cpuwait[CONFIG_SMP_NCPUS];
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* NuttX domain function prototypes
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-18 18:46:08 +01:00
|
|
|
void sim_cpu_pause(int cpu, volatile spinlock_t *wait,
|
|
|
|
volatile unsigned char *paused);
|
2016-02-10 21:58:21 +01:00
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Functions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: sim_idle_trampoline
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* This is a pthread task entry point. A (host) pthread is used to
|
|
|
|
* simulate a CPU. Multiple pthreads is a good analog to tasks running on
|
2016-02-11 19:16:19 +01:00
|
|
|
* multiple CPUs
|
2016-02-10 21:58:21 +01:00
|
|
|
*
|
|
|
|
* This function is simply a wrapper that sets the pthread specific data
|
|
|
|
* that presents the CPU number and then calls into the IDLE task entry
|
|
|
|
* point.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
2016-02-18 18:12:43 +01:00
|
|
|
* arg - Standard pthread argument
|
2016-02-10 21:58:21 +01:00
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* This function does not return
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
static void *sim_idle_trampoline(void *arg)
|
|
|
|
{
|
|
|
|
struct sim_cpuinfo_s *cpuinfo = (struct sim_cpuinfo_s *)arg;
|
2016-02-18 18:12:43 +01:00
|
|
|
sigset_t set;
|
2016-02-10 21:58:21 +01:00
|
|
|
int ret;
|
|
|
|
|
|
|
|
/* Set the CPU number zero for the CPU thread */
|
|
|
|
|
|
|
|
ret = pthread_setspecific(g_cpukey, (const void *)((uintptr_t)cpuinfo->cpu));
|
|
|
|
if (ret != 0)
|
|
|
|
{
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
2016-02-18 18:12:43 +01:00
|
|
|
/* Make sure the SIGUSR1 is not masked */
|
|
|
|
|
|
|
|
sigemptyset(&set);
|
|
|
|
sigaddset(&set, SIGUSR1);
|
|
|
|
|
|
|
|
ret = pthread_sigmask(SIG_UNBLOCK, &set, NULL);
|
|
|
|
if (ret < 0)
|
|
|
|
{
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
2016-02-17 20:35:23 +01:00
|
|
|
/* Let up_cpu_start() continue */
|
2016-02-12 00:18:28 +01:00
|
|
|
|
|
|
|
(void)pthread_mutex_unlock(&cpuinfo->mutex);
|
|
|
|
|
2016-02-10 21:58:21 +01:00
|
|
|
/* Give control to the IDLE task */
|
|
|
|
|
|
|
|
(void)cpuinfo->idletask(0, (char **)0);
|
|
|
|
|
|
|
|
/* The IDLE task will not return. This is just to keep the compiler happy */
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
2016-02-18 18:12:43 +01:00
|
|
|
/****************************************************************************
|
|
|
|
* Name: sim_handle_signal
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* This is the SIGUSR signal handler. It implements the core logic of
|
|
|
|
* up_cpu_pause() on the thread of execution the simulated CPU.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* arg - Standard sigaction arguments
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* None
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
static void sim_handle_signal(int signo, siginfo_t *info, void *context)
|
|
|
|
{
|
|
|
|
int cpu = (int)((uintptr_t)pthread_getspecific(g_cpukey));
|
|
|
|
|
|
|
|
/* We need to perform the actual tasking operations in the NuttX domain */
|
|
|
|
|
2016-02-18 18:46:08 +01:00
|
|
|
sim_cpu_pause(cpu, &g_sim_cpuwait[cpu], &g_sim_cpupaused[cpu]);
|
2016-02-18 18:12:43 +01:00
|
|
|
}
|
|
|
|
|
2016-02-10 21:58:21 +01:00
|
|
|
/****************************************************************************
|
|
|
|
* Public Functions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
2016-02-17 20:35:23 +01:00
|
|
|
* Name: sim_cpu0_initialize
|
2016-02-10 21:58:21 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Create the pthread-specific data key and set the indication of CPU0
|
|
|
|
* the the main thread.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* None
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* An integer index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
|
|
|
|
* corresponds to the currently executing CPU.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-17 20:35:23 +01:00
|
|
|
int sim_cpu0_initialize(void)
|
2016-02-10 21:58:21 +01:00
|
|
|
{
|
2016-02-18 18:12:43 +01:00
|
|
|
struct sigaction act;
|
|
|
|
sigset_t set;
|
2016-02-10 21:58:21 +01:00
|
|
|
int ret;
|
|
|
|
|
|
|
|
/* Create the pthread key */
|
|
|
|
|
|
|
|
ret = pthread_key_create(&g_cpukey, NULL);
|
|
|
|
if (ret != 0)
|
|
|
|
{
|
|
|
|
return -ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Set the CPU number zero for the main thread */
|
|
|
|
|
|
|
|
ret = pthread_setspecific(g_cpukey, (const void *)0);
|
|
|
|
if (ret != 0)
|
|
|
|
{
|
|
|
|
return -ret;
|
|
|
|
}
|
|
|
|
|
2016-02-18 18:12:43 +01:00
|
|
|
/* Register the common signal handler for all threads */
|
|
|
|
|
|
|
|
act.sa_sigaction = sim_handle_signal;
|
|
|
|
act.sa_flags = SA_SIGINFO;
|
|
|
|
sigemptyset(&act.sa_mask);
|
|
|
|
|
|
|
|
ret = sigaction(SIGUSR1, &act, NULL);
|
|
|
|
if (ret < 0)
|
|
|
|
{
|
|
|
|
return -errno;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Make sure the SIGUSR1 is not masked */
|
|
|
|
|
|
|
|
sigemptyset(&set);
|
|
|
|
sigaddset(&set, SIGUSR1);
|
|
|
|
|
|
|
|
ret = sigprocmask(SIG_UNBLOCK, &set, NULL);
|
|
|
|
if (ret < 0)
|
|
|
|
{
|
|
|
|
return -errno;
|
|
|
|
}
|
|
|
|
|
2016-02-10 21:58:21 +01:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2016-02-17 20:35:23 +01:00
|
|
|
* Name: up_cpu_index
|
2016-02-10 21:58:21 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Return an index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
|
|
|
|
* corresponds to the currently executing CPU.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* None
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* An integer index in the range of 0 through (CONFIG_SMP_NCPUS-1) that
|
|
|
|
* corresponds to the currently executing CPU.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-17 20:35:23 +01:00
|
|
|
int up_cpu_index(void)
|
2016-02-10 21:58:21 +01:00
|
|
|
{
|
|
|
|
void *value = pthread_getspecific(g_cpukey);
|
|
|
|
return (int)((uintptr_t)value);
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2016-02-17 20:35:23 +01:00
|
|
|
* Name: up_cpu_start
|
2016-02-10 21:58:21 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* In an SMP configution, only one CPU is initially active (CPU 0). System
|
|
|
|
* initialization occurs on that single thread. At the completion of the
|
|
|
|
* initialization of the OS, just before beginning normal multitasking,
|
|
|
|
* the additional CPUs would be started by calling this function.
|
|
|
|
*
|
2016-02-16 14:50:09 +01:00
|
|
|
* Each CPU is provided the entry point to is IDLE task when started. A
|
|
|
|
* TCB for each CPU's IDLE task has been initialized and placed in the
|
|
|
|
* CPU's g_assignedtasks[cpu] list. Not stack has been alloced or
|
|
|
|
* initialized.
|
|
|
|
*
|
|
|
|
* The OS initialization logic calls this function repeatedly until each
|
|
|
|
* CPU has been started, 1 through (CONFIG_SMP_NCPUS-1).
|
2016-02-10 21:58:21 +01:00
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* cpu - The index of the CPU being started. This will be a numeric
|
|
|
|
* value in the range of from one to (CONFIG_SMP_NCPUS-1). (CPU
|
|
|
|
* 0 is already active)
|
|
|
|
* idletask - The entry point to the IDLE task.
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* Zero on success; a negated errno value on failure.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-17 20:35:23 +01:00
|
|
|
int up_cpu_start(int cpu, main_t idletask)
|
2016-02-10 21:58:21 +01:00
|
|
|
{
|
|
|
|
struct sim_cpuinfo_s cpuinfo;
|
|
|
|
int ret;
|
|
|
|
|
|
|
|
/* Initialize the CPU info */
|
|
|
|
|
|
|
|
cpuinfo.cpu = cpu;
|
|
|
|
cpuinfo.idletask = idletask;
|
|
|
|
ret = pthread_mutex_init(&cpuinfo.mutex, NULL);
|
|
|
|
if (ret != 0)
|
|
|
|
{
|
2016-02-11 19:16:19 +01:00
|
|
|
return -ret; /* REVISIT: That is a host errno value. */
|
2016-02-10 21:58:21 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
/* Lock the mutex */
|
|
|
|
|
|
|
|
ret = pthread_mutex_lock(&cpuinfo.mutex);
|
|
|
|
if (ret != 0)
|
|
|
|
{
|
2016-02-11 19:16:19 +01:00
|
|
|
ret = -ret; /* REVISIT: This is a host errno value. */
|
2016-02-10 21:58:21 +01:00
|
|
|
goto errout_with_mutex;
|
|
|
|
}
|
|
|
|
|
2016-02-11 19:16:19 +01:00
|
|
|
/* Start the CPU emulation thread. This is analogous to starting the CPU
|
|
|
|
* in a multi-CPU hardware model.
|
|
|
|
*/
|
2016-02-10 21:58:21 +01:00
|
|
|
|
2016-02-18 18:12:43 +01:00
|
|
|
ret = pthread_create(&g_sim_cputhread[cpu], NULL, sim_idle_trampoline, &cpuinfo);
|
2016-02-10 21:58:21 +01:00
|
|
|
if (ret != 0)
|
|
|
|
{
|
2016-02-11 19:16:19 +01:00
|
|
|
ret = -ret; /* REVISIT: That is a host errno value. */
|
2016-02-10 21:58:21 +01:00
|
|
|
goto errout_with_lock;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Try to lock the mutex again. This will block until the pthread unlocks
|
|
|
|
* the mutex.
|
|
|
|
*/
|
|
|
|
|
|
|
|
ret = pthread_mutex_lock(&cpuinfo.mutex);
|
|
|
|
if (ret != 0)
|
|
|
|
{
|
2016-02-11 19:16:19 +01:00
|
|
|
ret = -ret; /* REVISIT: That is a host errno value. */
|
2016-02-10 21:58:21 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
errout_with_lock:
|
|
|
|
(void)pthread_mutex_unlock(&cpuinfo.mutex);
|
|
|
|
|
|
|
|
errout_with_mutex:
|
|
|
|
(void)pthread_mutex_destroy(&cpuinfo.mutex);
|
|
|
|
return ret;
|
|
|
|
}
|
2016-02-13 00:12:01 +01:00
|
|
|
|
|
|
|
/****************************************************************************
|
2016-02-17 20:35:23 +01:00
|
|
|
* Name: up_cpu_pause
|
2016-02-13 00:12:01 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Save the state of the current task at the head of the
|
2016-02-17 20:35:23 +01:00
|
|
|
* g_assignedtasks[cpu] task list and then pause task execution on the
|
|
|
|
* CPU.
|
2016-02-13 00:12:01 +01:00
|
|
|
*
|
|
|
|
* This function is called by the OS when the logic executing on one CPU
|
|
|
|
* needs to modify the state of the g_assignedtasks[cpu] list for another
|
|
|
|
* CPU.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* cpu - The index of the CPU to be stopped/
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* Zero on success; a negated errno value on failure.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-17 20:35:23 +01:00
|
|
|
int up_cpu_pause(int cpu)
|
2016-02-13 00:12:01 +01:00
|
|
|
{
|
2016-02-18 18:12:43 +01:00
|
|
|
/* Take the spinlock that will prevent the CPU thread from running */
|
|
|
|
|
|
|
|
g_sim_cpuwait[cpu] = SP_LOCKED;
|
|
|
|
|
|
|
|
/* Signal the CPU thread */
|
|
|
|
|
|
|
|
pthread_kill(g_sim_cputhread[cpu], SIGUSR1);
|
|
|
|
|
|
|
|
/* Spin, waiting for the thread to pause */
|
|
|
|
|
|
|
|
while (!g_sim_cpupaused[cpu])
|
|
|
|
{
|
|
|
|
pthread_yield();
|
|
|
|
}
|
|
|
|
|
2016-02-13 00:12:01 +01:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2016-02-17 20:35:23 +01:00
|
|
|
* Name: up_cpu_resume
|
2016-02-13 00:12:01 +01:00
|
|
|
*
|
|
|
|
* Description:
|
2016-02-17 20:35:23 +01:00
|
|
|
* Restart the cpu after it was paused via up_cpu_pause(), restoring the
|
|
|
|
* state of the task at the head of the g_assignedtasks[cpu] list, and
|
|
|
|
* resume normal tasking.
|
2016-02-13 00:12:01 +01:00
|
|
|
*
|
2016-02-17 20:35:23 +01:00
|
|
|
* This function is called after up_cpu_pause in order resume operation of
|
2016-02-13 00:12:01 +01:00
|
|
|
* the CPU after modifying its g_assignedtasks[cpu] list.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* cpu - The index of the CPU being re-started.
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* Zero on success; a negated errno value on failure.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2016-02-17 20:35:23 +01:00
|
|
|
int up_cpu_resume(int cpu)
|
2016-02-13 00:12:01 +01:00
|
|
|
{
|
2016-02-18 18:12:43 +01:00
|
|
|
/* Release the spinlock that will alloc the CPU thread to continue */
|
|
|
|
|
|
|
|
g_sim_cpuwait[cpu] = SP_UNLOCKED;
|
2016-02-13 00:12:01 +01:00
|
|
|
return 0;
|
|
|
|
}
|