diff --git a/.gitignore b/.gitignore index ea416fae488c..f1cff78604c4 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ Make.dep *~ Images/*.bin Images/*.px4 +Images/*.exe nuttx/Make.defs nuttx/setenv.sh nuttx/arch/arm/include/board @@ -40,3 +41,5 @@ nsh_romfsimg.h cscope.out .configX-e nuttx-export.zip +nuttx/arch/sim/include/board +nuttx/arch/sim/src/board diff --git a/Makefile b/Makefile index 801256cfec93..afad4eefc0f7 100644 --- a/Makefile +++ b/Makefile @@ -6,7 +6,6 @@ # project to slim down and simply generate PX4 link kits via the NuttX # 'make export' mechanism. # -# # # Some useful paths. @@ -42,6 +41,7 @@ endif FIRMWARE_BUNDLE = $(IMAGE_DIR)/$(TARGET).px4 FIRMWARE_BINARY = $(IMAGE_DIR)/$(TARGET).bin FIRMWARE_PROTOTYPE = $(IMAGE_DIR)/$(TARGET).prototype +FIRMWARE_EXEC = $(IMAGE_DIR)/$(TARGET).exe # # Debugging @@ -49,7 +49,11 @@ FIRMWARE_PROTOTYPE = $(IMAGE_DIR)/$(TARGET).prototype MQUIET = --no-print-directory #MQUIET = --print-directory -all: $(FIRMWARE_BUNDLE) +ifeq ($(TARGET),px4fmu_sim) +all: $(FIRMWARE_EXEC) +else +all: $(FIRMWARE_BUNDLE) +endif # # Generate a wrapped .px4 file from the built binary @@ -68,6 +72,15 @@ $(FIRMWARE_BINARY): configure_$(TARGET) setup_$(TARGET) @make -C $(NUTTX_SRC) -r $(MQUIET) all @cp $(NUTTX_SRC)/nuttx.bin $@ +# +# Build the firmware executable. +# +.PHONY: $(FIRMWARE_EXEC) +$(FIRMWARE_EXEC): configure_$(TARGET) setup_$(TARGET) + @echo Building $@ + @make -C $(NUTTX_SRC) -r $(MQUIET) all + @cp $(NUTTX_SRC)/nuttx $@ + # # The 'configure' targets select one particular firmware configuration # and makes it current. @@ -86,6 +99,13 @@ endif @cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4io/io @echo px4io > $(CONFIGURED) +configure_px4fmu_sim: +ifneq ($(TARGET),px4fmu_sim) + @make -C $(PX4BASE) distclean +endif + @cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4fmu_sim/nsh + @echo px4fmu_sim > $(CONFIGURED) + # # Per-configuration additional targets # @@ -96,6 +116,10 @@ setup_px4fmu: setup_px4io: +setup_px4fmu_sim: + @echo Generating ROMFS + @make -C $(ROMFS_SRC) all + # # Firmware uploading. # diff --git a/ROMFS/Makefile b/ROMFS/Makefile index ec4221b93aad..f162c955cbb7 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -10,12 +10,14 @@ # SRCROOT ?= $(dir $(lastword $(MAKEFILE_LIST))) BUILDROOT ?= $(SRCROOT)/img -ROMFS_HEADER ?= $(SRCROOT)/../nuttx/configs/px4fmu/include/nsh_romfsimg.h +ROMFS_HEADER ?= $(SRCROOT)/../nuttx/configs/px4fmu/include/nsh_romfsimg.h +ROMFS_SIM_HEADER ?= $(SRCROOT)/../nuttx/configs/px4fmu_sim/include/nsh_romfsimg.h # # List of files to install in the ROMFS, specified as ~ # ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ + $(SRCROOT)/scripts/rc.sim~init.d/rc.sim \ $(SRCROOT)/scripts/rc.sensors~init.d/rc.sensors \ $(SRCROOT)/scripts/rc.logging~init.d/rc.logging \ $(SRCROOT)/scripts/rc.standalone~init.d/rc.standalone \ @@ -72,12 +74,16 @@ ROMFS_WORKDIR = $(BUILDROOT)/romfs # # Convenience target for rebuilding the ROMFS header # -all: $(ROMFS_HEADER) +all: $(ROMFS_HEADER) $(ROMFS_SIM_HEADER) $(ROMFS_HEADER): $(ROMFS_IMG) $(dir $(ROMFS_HEADER)) @echo Generating the ROMFS header... @(cd $(dir $(ROMFS_IMG)) && xxd -i $(notdir $(ROMFS_IMG))) > $@ +$(ROMFS_SIM_HEADER): $(ROMFS_HEADER) + @echo Generating the ROMFS sim header... + @cp $^ $@ + $(ROMFS_IMG): $(ROMFS_WORKDIR) @echo Generating the ROMFS image... @genromfs -f $@ -d $(ROMFS_WORKDIR) -V "NSHInitVol" diff --git a/ROMFS/scripts/rc.sim b/ROMFS/scripts/rc.sim new file mode 100644 index 000000000000..7c2987d48621 --- /dev/null +++ b/ROMFS/scripts/rc.sim @@ -0,0 +1,40 @@ +#!nsh +# +# Startup script for simulation configuration. +# + +echo "[init] doing simulation startup..." + +echo "[init] uORB" +uorb start + +echo "[init] sensors" +#bma180 start +#l3gd20 start +#mpu6000 start +#hmc5883 start +#ms5611 start + +sensors start + +echo "[init] mavlink" +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +echo "[init] commander" +commander start + +echo "[init] attitude control" +#attitude_estimator_ekf start +#multirotor_att_control start + +#echo "[init] starting PWM output" +#fmu mode_pwm +#mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +echo "[init] simulation startup done" + +echo "[init] uORB" +uorb test + + diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 4152494e0e9b..4237dd1873b0 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -112,6 +112,24 @@ else echo "[init] PX4IO not detected" fi + # + # Are we running a simulation? + # + uname SIM + echo $? + if uname SIM + then + set BOARD PX4FMU_SIM + if [ -f /etc/init.d/rc.sim ] + then + echo "[init] reading /etc/init.d/rc.sim" + usleep 500 + sh /etc/init.d/rc.sim + fi + else + echo "[init] simulation not detected" + fi + # # Looks like we are stand-alone # diff --git a/apps/nshlib/nsh_consolemain.c b/apps/nshlib/nsh_consolemain.c index 6b51be4705dc..935b213a51bb 100644 --- a/apps/nshlib/nsh_consolemain.c +++ b/apps/nshlib/nsh_consolemain.c @@ -122,7 +122,12 @@ int nsh_consolemain(int argc, char *argv[]) #endif /* Then enter the command line parsing loop */ - +#ifdef CONFIG_ARCH_SIM + for (;;) + { + sleep(1); + } +#else for (;;) { /* For the case of debugging the USB console... dump collected USB trace data */ @@ -159,6 +164,7 @@ int nsh_consolemain(int argc, char *argv[]) nsh_exit(&pstate->cn_vtbl, 1); } } +#endif /* Clean up */ diff --git a/apps/sim/drivers/Makefile b/apps/sim/drivers/Makefile new file mode 100644 index 000000000000..e73a6a79c043 --- /dev/null +++ b/apps/sim/drivers/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# SIM driver support code +# +# Modules in this directory are compiled for all SIM targets. +# + +INCLUDES = $(TOPDIR)/arch/sim/src $(TOPDIR)/arch/sim/include + +include $(APPDIR)/mk/app.mk diff --git a/apps/sim/drivers/drv_hrt.c b/apps/sim/drivers/drv_hrt.c new file mode 100644 index 000000000000..7dd4670f3994 --- /dev/null +++ b/apps/sim/drivers/drv_hrt.c @@ -0,0 +1,444 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file drv_hrt.c + * + * High-resolution timer callouts and timekeeping. + * + * This can use any general or advanced STM32 timer. + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + * + * We don't use the NuttX STM32 driver per se; rather, we + * claim the timer and then drive it directly. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +/*#include */ +#include + +/*#include "chip.h"*/ +/*#include "up_internal.h"*/ +/*#include "up_arch.h"*/ + +/*#include "stm32_internal.h"*/ +/*#include "stm32_gpio.h"*/ +/*#include "stm32_tim.h"*/ + +#ifdef CONFIG_HRT_TIMER + +/* + * Minimum/maximum deadlines. + * + * These are suitable for use with a 16-bit timer/counter clocked + * at 1MHz. The high-resolution timer need only guarantee that it + * not wrap more than once in the 50ms period for absolute time to + * be consistently maintained. + * + * The minimum deadline must be such that the time taken between + * reading a time and writing a deadline to the timer cannot + * result in missing the deadline. + */ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + +/* + * Period of the free-running counter, in microseconds. + */ +#define HRT_COUNTER_PERIOD 65536 + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint16_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint16_t latency_actual; + +/* latency histogram */ +#define LATENCY_BUCKET_COUNT 8 +static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +/*static int hrt_tim_isr(int irq, void *context);*/ +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +/* + * Initialise the timer we are going to use. + * + * We expect that we'll own one of the reduced-function STM32 general + * timers, and that we can use channel 1 in compare mode. + */ +static void +hrt_tim_init(void) +{ + /* enable interrupts */ + /*up_enable_irq(HRT_TIMER_VECTOR);*/ +} + +/* + * Handle the compare interupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +/*static int*/ +/*hrt_tim_isr(int irq, void *context)*/ +/*{*/ + /*uint32_t status;*/ + + /*[> grab the timer for latency tracking purposes <]*/ + /*latency_actual = rCNT;*/ + + /*[> copy interrupt status <]*/ + /*status = rSR;*/ + + /*[> ack the interrupts we just read <]*/ + /*rSR = ~status;*/ + + /*[> was this a timer tick? <]*/ + /*if (status & SR_INT_HRT) {*/ + + /*[> do latency calculations <]*/ + /*hrt_latency_update();*/ + + /*[> run any callouts that have met their deadline <]*/ + /*hrt_call_invoke();*/ + + /*[> and schedule the next interrupt <]*/ + /*hrt_call_reschedule();*/ + /*}*/ + + /*return OK;*/ +/*}*/ + +/* + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime; + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + return ts_to_abstime(&ts); +} + +/* + * Convert a timespec to absolute time + */ +hrt_abstime +ts_to_abstime(struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} + +/* + * Convert absolute time to a timespec. + */ +void +abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} + +/* + * Initalise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); +} + +/* + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/* + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/* + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = irqsave(); + + /* if the entry is currently queued, remove it */ + if (entry->deadline != 0) + sq_rem(&entry->link, &callout_queue); + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + irqrestore(flags); +} + +/* + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/* + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = irqsave(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + irqrestore(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) + break; + + if (call->deadline > now) + break; + + sq_rem(&call->link, &callout_queue); + //lldbg("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + call->deadline = deadline + call->period; + hrt_call_enter(call); + } + } +} + +/* + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } + + //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + /*rCCR_HRT = latency_baseline = deadline & 0xffff;*/ +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + + +#endif /* CONFIG_HRT_TIMER */ diff --git a/apps/sim/drivers/drv_pwm_servo.c b/apps/sim/drivers/drv_pwm_servo.c new file mode 100644 index 000000000000..313d8e01f168 --- /dev/null +++ b/apps/sim/drivers/drv_pwm_servo.c @@ -0,0 +1,32 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ diff --git a/apps/sim/drivers/tone_alarm/Makefile b/apps/sim/drivers/tone_alarm/Makefile new file mode 100644 index 000000000000..a91043e9590d --- /dev/null +++ b/apps/sim/drivers/tone_alarm/Makefile @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# Tone alarm driver +# + +APPNAME = tone_alarm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 +INCLUDES = $(TOPDIR)/arch/sim/src + +include $(APPDIR)/mk/app.mk diff --git a/apps/sim/drivers/tone_alarm/tone_alarm.cpp b/apps/sim/drivers/tone_alarm/tone_alarm.cpp new file mode 100644 index 000000000000..d365523eb751 --- /dev/null +++ b/apps/sim/drivers/tone_alarm/tone_alarm.cpp @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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. + * + ****************************************************************************/ + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); + +int +tone_alarm_main(int argc, char *argv[]) +{ + return 0; +} diff --git a/apps/sim/sensors/Makefile b/apps/sim/sensors/Makefile new file mode 100644 index 000000000000..526fb0164f00 --- /dev/null +++ b/apps/sim/sensors/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# Makefile to build the sensor data collector +# + +APPNAME = sensors +PRIORITY = SCHED_PRIORITY_MAX-5 +STACKSIZE = 4096 + +CXXSRCS = sensors.cpp +CSRCS = sensor_params.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/sim/sensors/sensor_params.c b/apps/sim/sensors/sensor_params.c new file mode 100644 index 000000000000..fd8a6602ac00 --- /dev/null +++ b/apps/sim/sensors/sensor_params.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file sensor_params.c + * + * Parameters defined by the sensors task. + */ + +#include + +#include + +PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); +PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); +PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); +PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC2_MIN, 1000); +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC3_MIN, 1000); +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC3_MAX, 2000); +PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC4_MIN, 1000); +PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC4_MAX, 2000); +PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC5_MIN, 1000); +PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC5_MAX, 2000); +PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC6_MIN, 1000); +PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC6_MAX, 2000); +PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC7_MIN, 1000); +PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC7_MAX, 2000); +PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC8_MIN, 1000); +PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC8_MAX, 2000); +PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA + +/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); + +PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); +PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); +PARAM_DEFINE_INT32(RC_MAP_YAW, 4); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); +PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); +PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); + +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f); + diff --git a/apps/sim/sensors/sensors.cpp b/apps/sim/sensors/sensors.cpp new file mode 100644 index 000000000000..78fc943ea14b --- /dev/null +++ b/apps/sim/sensors/sensors.cpp @@ -0,0 +1,331 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file sensors.cpp + * @author Lorenz Meier + * + * Sensor readout process. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// Constants +#define BAT_VOL_INITIAL 12.f + +// Forward Declarations +class Sensors; + +// Globals +namespace sensors { + Sensors * g_sensors; +}; + +class Sensors +{ +private: + int _sensors_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _publishing; /**< if true, we are publishing sensor data */ + orb_advert_t _sensor_pub; /**< combined sensor data topic */ + orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _rc_pub; /**< raw r/c control topic */ +public: + Sensors() : + _sensors_task(-1), + _task_should_exit(false), + _publishing(true), + _sensor_pub(-1), + _manual_control_pub(-1), + _rc_pub(-1) + { + } + ~Sensors() + { + if (_sensors_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_sensors_task); + break; + } + } while (_sensors_task != -1); + } + + sensors::g_sensors = nullptr; + } + static void task_main_trampoline(int argc, char *argv[]) + { + sensors::g_sensors->task_main(); + } + int start() + { + ASSERT(_sensors_task == -1); + + /* start the task */ + _sensors_task = task_spawn("sensors_task", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 6000, /* XXX may be excesssive */ + (main_t)&Sensors::task_main_trampoline, + nullptr); + + if (_sensors_task < 0) { + warn("task start failed"); + return -errno; + } + return OK; + } + void task_main() + { + + /* inform about start */ + printf("[sensors] Initializing..\n"); + fflush(stdout); + + /* start individual sensors */ + //accel_init(); + //gyro_init(); + //mag_init(); + //baro_init(); + //adc_init(); + + /* + * do subscriptions + */ + //_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + //_accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + //_mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + //_rc_sub = orb_subscribe(ORB_ID(input_rc)); + //_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + //_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + //_params_sub = orb_subscribe(ORB_ID(parameter_update)); + //_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + //orb_set_interval(_vstatus_sub, 200); + + /* + * do advertisements + */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + raw.timestamp = 0; hrt_absolute_time(); + raw.battery_voltage_v = BAT_VOL_INITIAL; + raw.adc_voltage_v[0] = 0.9f; + raw.adc_voltage_v[1] = 0.0f; + raw.adc_voltage_v[2] = 0.0f; + raw.battery_voltage_counter = 0; + raw.battery_voltage_valid = false; + + /* get a set of initial values */ + //accel_poll(raw); + //gyro_poll(raw); + //mag_poll(raw); + //baro_poll(raw); + + //parameter_update_poll(true [> forced <]); + + /* advertise the sensor_combined topic and make the initial publication */ + _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + + /* advertise the manual_control topic */ + struct manual_control_setpoint_s manual_control; + manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; + manual_control.roll = 0.0f; + manual_control.pitch = 0.0f; + manual_control.yaw = 0.0f; + manual_control.throttle = 0.0f; + manual_control.aux1_cam_pan_flaps = 0.0f; + manual_control.aux2_cam_tilt = 0.0f; + manual_control.aux3_cam_zoom = 0.0f; + manual_control.aux4_cam_roll = 0.0f; + + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + + /* advertise the rc topic */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + } + + /* wakeup source(s) */ + //struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + //fds[0].fd = _gyro_sub; + //fds[0].events = POLLIN; + + while (!_task_should_exit) { + + + /* wait for up to 500ms for data */ + //int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + //if (pret == 0) + //continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + //if (pret < 0) { + //warn("poll error %d, %d", pret, errno); + //continue; + //} + + //perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + //vehicle_status_poll(); + + /* check parameters for updates */ + //parameter_update_poll(); + + /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ + raw.timestamp = hrt_absolute_time(); + //printf("\nhrt time: %d\n", raw.timestamp); + + /* copy most recent sensor data */ + //gyro_poll(raw); + //accel_poll(raw); + //mag_poll(raw); + //baro_poll(raw); + + /* check battery voltage */ + //adc_poll(raw); + + /* Inform other processes that new data is available to copy */ + if (_publishing) + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + +#ifdef CONFIG_HRT_PPM + /* Look for new r/c input data */ + //ppm_poll(); +#endif + + //perf_end(_loop_perf); + + // sleep to allow simulation to switch tasks + usleep(1); + } + + printf("[sensors] exiting.\n"); + + _sensors_task = -1; + _exit(0); + } + +}; + +/** + * Sensor app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int sensors_main(int argc, char *argv[]); + +int sensors_main(int argc, char *argv[]) +{ + if (argc != 2 || argv[1]==NULL) + errx(1, "usage: sensors {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (sensors::g_sensors != nullptr) + errx(1, "sensors task already running"); + + sensors::g_sensors = new Sensors; + if (sensors::g_sensors == nullptr) + errx(1, "sensors task alloc failed"); + + if (OK != sensors::g_sensors->start()) { + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + err(1, "sensors task start failed"); + } + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (sensors::g_sensors == nullptr) + errx(1, "sensors task not running"); + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (sensors::g_sensors) { + errx(0, "task is running"); + } else { + errx(1, "task is not running"); + } + } + + errx(1, "unrecognized command"); +} diff --git a/apps/systemcmds/uname/Makefile b/apps/systemcmds/uname/Makefile new file mode 100644 index 000000000000..1d11e425065d --- /dev/null +++ b/apps/systemcmds/uname/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# 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 PX4 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. +# +############################################################################ + +# +# realtime system performance display +# + +APPNAME = uname +PRIORITY = SCHED_PRIORITY_DEFAULT - 10 +STACKSIZE = 3000 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/uname/uname.c b/apps/systemcmds/uname/uname.c new file mode 100644 index 000000000000..91aef8edb23b --- /dev/null +++ b/apps/systemcmds/uname/uname.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 PX4 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. + * + ****************************************************************************/ + +/** + * @file uname.c + * Tool similar to UNIX uname command + */ + +#include + +/** + * Start the uname application. + */ +__EXPORT int uname_main(int argc, char *argv[]); + +int uname_main(int argc, char *argv[]) +{ + if (argc != 2) return 0; +#if defined CONFIG_ARCH_SIM + static char arch[] = "SIM"; +#elif defined CONFIG_ARCH_ARM + static char arch[] = "ARM"; +#else + static char arch[] = "UNKNOWN"; +#endif + return (strcmp(argv[1],arch) != 0); +} diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index 5778c552e237..5705655adbf0 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -57,5 +57,12 @@ CSRCS += systemlib.c \ pid/pid.c \ geo/geo.c endif +ifeq ($(TARGET),px4fmu_sim) +CSRCS += systemlib.c \ + pid/pid.c \ + geo/geo.c +endif + + include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/cpuload.c b/apps/systemlib/cpuload.c index 20b711fa66e0..f596b5e96210 100644 --- a/apps/systemlib/cpuload.c +++ b/apps/systemlib/cpuload.c @@ -111,6 +111,7 @@ void cpuload_initialize_once() void sched_note_start(FAR _TCB *tcb) { + if (!system_load.initialized) return; /* search first free slot */ int i; @@ -130,6 +131,7 @@ void sched_note_start(FAR _TCB *tcb) void sched_note_stop(FAR _TCB *tcb) { + if (!system_load.initialized) return; int i; for (i = 1; i < CONFIG_MAX_TASKS; i++) { @@ -147,6 +149,7 @@ void sched_note_stop(FAR _TCB *tcb) void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) { + if (!system_load.initialized) return; uint64_t new_time = hrt_absolute_time(); /* Kind of inefficient: find both tasks and update times */ diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 6746e8ff3624..132b506395c6 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -331,8 +331,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d // TO DO - this is messed up and won't compile float start_disp_x = radius * sin(arc_start_bearing); float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_x = radius * sin(_wrap_pi(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrap_pi(arc_start_bearing + arc_sweep)); float lon_start = lon_now + start_disp_x / 111111.0d; float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; float lon_end = lon_now + end_disp_x / 111111.0d; @@ -353,7 +353,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrap_pi(crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 532e54b8e0ec..c98a4b56a9ce 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -60,6 +60,8 @@ #include +#include + #include "uORB.h" /** @@ -636,20 +638,37 @@ test_note(const char *fmt, ...) ORB_DECLARE(sensor_combined); -int -test() +int publisher_thread(int argc, char * argv[]) { - struct orb_test t, u; + struct orb_test t; int pfd, sfd; - bool updated; t.val = 0; pfd = orb_advertise(ORB_ID(orb_test), &t); - if (pfd < 0) return test_fail("advertise failed: %d", errno); test_note("publish handle 0x%08x", pfd); + test_note("publishing val 0"); + + sleep(1); + + t.val = 2; + test_note("publishing val 2"); + + if (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) + return test_fail("publish failed"); + + close(pfd); + test_note("publisher complete"); + return 0; +} + +int subscriber_thread(int argc, char * argv[]) +{ + struct orb_test u; + int sfd; + bool updated; sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) @@ -657,12 +676,13 @@ test() test_note("subscribe fd %d", sfd); u.val = 1; + test_note("waiting for first value"); if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) return test_fail("copy(1) failed: %d", errno); - if (u.val != t.val) - return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + if (u.val != 0) + return test_fail("copy(1) mismatch: %d expected %d", u.val, 0); if (OK != orb_check(sfd, &updated)) return test_fail("check(1) failed"); @@ -670,11 +690,8 @@ test() if (updated) return test_fail("spurious updated flag"); - t.val = 2; - test_note("try publish"); - - if (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) - return test_fail("publish failed"); + sleep(1); + test_note("waiting for second value"); if (OK != orb_check(sfd, &updated)) return test_fail("check(2) failed"); @@ -685,11 +702,38 @@ test() if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) return test_fail("copy(2) failed: %d", errno); - if (u.val != t.val) - return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + if (u.val != 2) + return test_fail("copy(2) mismatch: %d expected %d", u.val, 2); orb_unsubscribe(sfd); - close(pfd); + + test_note("PASS"); + return 0; +} + +int +test() +{ + sleep(1); + test_note("beginning uorb test"); + + task_spawn("test_publisher_task", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4096, + publisher_thread, + NULL); + + sleep(0.5); + + task_spawn("test_subscriber_task", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4096, + subscriber_thread, + NULL); + + #if 0 /* this is a hacky test that exploits the sensors app to test rate-limiting */ @@ -733,8 +777,6 @@ test() orb_unsubscribe(sfd); #endif - - return test_note("PASS"); } int diff --git a/nuttx/arch/sim/Kconfig b/nuttx/arch/sim/Kconfig new file mode 100644 index 000000000000..331f61c79e2e --- /dev/null +++ b/nuttx/arch/sim/Kconfig @@ -0,0 +1,103 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_SIM +comment "Simulation Configuration Options" + +config SIM_M32 + bool "Build 32-bit simulation on 64-bit machine" + default n + ---help--- + Simulation context switching is based on logic like setjmp and longjmp. This + context switching is only available for 32-bit targets. On 64-bit machines, + this context switching will fail. + + The workaround on 64-bit machines for now is to build for a 32-bit target on the + 64-bit machine. The workaround for this issue has been included in NuttX 6.15 and + beyond. For thoses versions, you must add SIM_M32=y to the .config file in + order to enable building a 32-bit image on a 64-bit platform. + +config SIM_WALLTIME + bool "Execution simulation in near real-time" + default n + ---help--- + NOTE: In order to facility fast testing, the sim target's IDLE loop, by default, + calls the system timer "interrupt handler" as fast as possible. As a result, there + really are no noticeable delays when a task sleeps. However, the task really does + sleep -- but the time scale is wrong. If you want behavior that is closer to + normal timing, then you can define CONFIG_SIM_WALLTIME=y in your configuration + file. This configuration setting will cause the sim target's IDLE loop to delay + on each call so that the system "timer interrupt" is called at a rate approximately + correct for the system timer tick rate. With this definition in the configuration, + sleep() behavior is more or less normal. + +config SIM_LCDDRIVER + bool "Build a simulated LCD driver" + default y + depends on NX && NX_LCDDRIVER + ---help--- + Build a simulated LCD driver" + +config SIM_FRAMEBUFFER + bool "Build a simulated frame buffer driver" + default y + depends on NX && !NX_LCDDRIVER + ---help--- + Build a simulated frame buffer driver" + +config SIM_X11FB + bool "Use X11 window" + default n + depends on NX && FB_CMAP + ---help--- + Use an X11 graphics window to simulate the graphics device" + +config SIM_X11NOSHM + bool "Don't use shared memory with X11" + default n + depends on SIM_X11FB + ---help--- + Don't use shared memory with the X11 graphics device emulation." + +config SIM_FBHEIGHT + int "Display height" + default 240 + depends on NX + ---help--- + Simulated display height. Default: 240 + +config SIM_FBWIDTH + int "Display width" + default 320 if SIM_LCDDRIVER + default 480 if SIM_FRAMEBUFFER + depends on NX + ---help--- + Simulated width of the display. Default: 320 or 480 + +config SIM_FBBPP + int "Pixel depth in bits" + default 8 + depends on NX + ---help--- + Pixel depth in bits. Valid choices are 4, 8, 16, 24, or 32. + If you use the X11 display emulation, the selected BPP must match the BPP + of your graphics hardware (probably 32 bits). Default: 8 + +config SIM_TOUCHSCREEN + bool "Support an X11 mouse-based touchscreen emulation" + default n + depends on SIM_X11FB && INPUT + ---help--- + Support an X11 mouse-based touchscreen emulation. Also needs INPUT=y + +config SIM_TCNWAITERS + bool "Maximum number poll() waiters" + default 4 + depends on !POLL_DISABLE && SIM_TOUCHSCREEN + ---help--- + The maximum number of threads that can be waiting on poll() for a touchscreen event. + Default: 4 + +endif diff --git a/nuttx/arch/sim/include/arch.h b/nuttx/arch/sim/include/arch.h new file mode 100644 index 000000000000..5ec8022b2703 --- /dev/null +++ b/nuttx/arch/sim/include/arch.h @@ -0,0 +1,80 @@ +/************************************************************ + * arch.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************/ + +/* This file should never be included directed but, rather, + * only indirectly through nuttx/arch.h + */ + +#ifndef __ARCH_ARCH_H +#define __ARCH_ARCH_H + +/************************************************************ + * Included Files + ************************************************************/ + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Inline functions + ************************************************************/ + +/************************************************************ + * Public Types + ************************************************************/ + +/************************************************************ + * Public Variables + ************************************************************/ + +/************************************************************ + * Public Function Prototypes + ************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __ARCH_ARCH_H */ + diff --git a/nuttx/arch/sim/include/irq.h b/nuttx/arch/sim/include/irq.h new file mode 100644 index 000000000000..ffc325790c0b --- /dev/null +++ b/nuttx/arch/sim/include/irq.h @@ -0,0 +1,106 @@ +/************************************************************ + * irq.h + * + * Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************/ + +/* This file should never be included directed but, rather, + * only indirectly through nuttx/irq.h + */ + +#ifndef __ARCH_IRQ_H +#define __ARCH_IRQ_H + +/************************************************************ + * Included Files + ************************************************************/ + +/************************************************************ + * Definitions + ************************************************************/ + +#define NR_IRQS 0 + +/************************************************************ + * Public Types + ************************************************************/ + +/* This struct defines the way the registers are stored */ + +#ifndef __ASSEMBLY__ +struct xcptcontext +{ + void *sigdeliver; /* Actual type is sig_deliver_t */ + + /* Storage order: %ebx, $esi, %edi, %ebp, sp, and return PC */ + + int regs[6]; +}; +#endif + +/************************************************************ + * Inline functions + ************************************************************/ + +#ifndef __ASSEMBLY__ +static inline irqstate_t irqsave(void) +{ + return 0; +} + +static inline void irqrestore(irqstate_t flags) +{ +} +#endif + +/************************************************************ + * Public Variables + ************************************************************/ + +/************************************************************ + * Public Function Prototypes + ************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __ARCH_IRQ_H */ + diff --git a/nuttx/arch/sim/include/limits.h b/nuttx/arch/sim/include/limits.h new file mode 100644 index 000000000000..9aa36b1d09b8 --- /dev/null +++ b/nuttx/arch/sim/include/limits.h @@ -0,0 +1,86 @@ +/************************************************************ + * arch/sim/include/limits.h + * + * Copyright (C) 2007, 2009, 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************/ + +#ifndef __ARCH_SIM_INCLUDE_LIMITS_H +#define __ARCH_SIM_INCLUDE_LIMITS_H + +/************************************************************ + * Included Files + ************************************************************/ + +/************************************************************ + * Definitions + ************************************************************/ + +#define CHAR_BIT 8 +#define SCHAR_MIN (-128) +#define SCHAR_MAX 127 +#define UCHAR_MAX 255 + +/* These could be different on machines where char is unsigned */ + +#ifdef __CHAR_UNSIGNED__ +#define CHAR_MIN 0 +#define CHAR_MAX UCHAR_MAX +#else +#define CHAR_MIN SCHAR_MIN +#define CHAR_MAX SCHAR_MAX +#endif + +#define SHRT_MIN (-32768) +#define SHRT_MAX 32767 +#define USHRT_MAX 65535 + +#define INT_MIN (-2147483648) +#define INT_MAX 2147483647 +#define UINT_MAX 4294967295 + +/* These change on 32-bit and 64-bit platforms */ + +#define LONG_MIN (-2147483648L) +#define LONG_MAX 2147483647L +#define ULONG_MAX 4294967295UL + +#define LLONG_MIN (-9223372036854775808LL) +#define LLONG_MAX 9223372036854775807LL +#define ULLONG_MAX 18446744073709551615ULL + +/* A pointer is 4 bytes */ + +#define PTR_MIN (-2147483648) +#define PTR_MAX 2147483647 +#define UPTR_MAX 4294967295 + +#endif /* __ARCH_SIM_INCLUDE_LIMITS_H */ diff --git a/nuttx/arch/sim/include/math.h b/nuttx/arch/sim/include/math.h new file mode 100644 index 000000000000..3e7b0a25448e --- /dev/null +++ b/nuttx/arch/sim/include/math.h @@ -0,0 +1,542 @@ + +/* Declarations for math functions. + Copyright (C) 1991-1993, 1995-1999, 2001, 2002, 2004, 2006, 2009, 2011 + Free Software Foundation, Inc. + This file is part of the GNU C Library. + + The GNU C Library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + The GNU C Library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the GNU C Library; if not, write to the Free + Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA + 02111-1307 USA. */ + +/* + * ISO C99 Standard: 7.12 Mathematics + */ + +#ifndef _MATH_H +#define _MATH_H 1 + +#include + +__BEGIN_DECLS + +/* Get machine-dependent HUGE_VAL value (returned on overflow). + On all IEEE754 machines, this is +Infinity. */ +#include +#ifdef __USE_ISOC99 +# include +# include + +/* Get machine-dependent INFINITY value. */ +# include + +/* Get machine-dependent NAN value (returned for some domain errors). */ +# include +#endif /* __USE_ISOC99 */ + +/* Get general and ISO C99 specific information. */ +//#include + +/* The file contains the prototypes for all the + actual math functions. These macros are used for those prototypes, + so we can easily declare each function as both `name' and `__name', + and can declare the float versions `namef' and `__namef'. */ + +#define __MATHCALL(function,suffix, args) \ + __MATHDECL (_Mdouble_,function,suffix, args) +#define __MATHDECL(type, function,suffix, args) \ + __MATHDECL_1(type, function,suffix, args); \ + __MATHDECL_1(type, __CONCAT(__,function),suffix, args) +#define __MATHCALLX(function,suffix, args, attrib) \ + __MATHDECLX (_Mdouble_,function,suffix, args, attrib) +#define __MATHDECLX(type, function,suffix, args, attrib) \ + __MATHDECL_1(type, function,suffix, args) __attribute__ (attrib); \ + __MATHDECL_1(type, __CONCAT(__,function),suffix, args) __attribute__ (attrib) +#define __MATHDECL_1(type, function,suffix, args) \ + extern type __MATH_PRECNAME(function,suffix) args __THROW + +#define _Mdouble_ double +#define __MATH_PRECNAME(name,r) __CONCAT(name,r) +#define _Mdouble_BEGIN_NAMESPACE __BEGIN_NAMESPACE_STD +#define _Mdouble_END_NAMESPACE __END_NAMESPACE_STD +#include +#undef _Mdouble_ +#undef _Mdouble_BEGIN_NAMESPACE +#undef _Mdouble_END_NAMESPACE +#undef __MATH_PRECNAME + +#if defined __USE_MISC || defined __USE_ISOC99 + + +/* Include the file of declarations again, this time using `float' + instead of `double' and appending f to each function name. */ + +# ifndef _Mfloat_ +# define _Mfloat_ float +# endif +# define _Mdouble_ _Mfloat_ +# ifdef __STDC__ +# define __MATH_PRECNAME(name,r) name##f##r +# else +# define __MATH_PRECNAME(name,r) name/**/f/**/r +# endif +# define _Mdouble_BEGIN_NAMESPACE __BEGIN_NAMESPACE_C99 +# define _Mdouble_END_NAMESPACE __END_NAMESPACE_C99 +# include +# undef _Mdouble_ +# undef _Mdouble_BEGIN_NAMESPACE +# undef _Mdouble_END_NAMESPACE +# undef __MATH_PRECNAME + +# if (__STDC__ - 0 || __GNUC__ - 0) \ + && (!(defined __NO_LONG_DOUBLE_MATH && defined _LIBC) \ + || defined __LDBL_COMPAT) +# ifdef __LDBL_COMPAT + +# ifdef __USE_ISOC99 +extern float __nldbl_nexttowardf (float __x, long double __y) + __THROW __attribute__ ((__const__)); +# ifdef __REDIRECT_NTH +extern float __REDIRECT_NTH (nexttowardf, (float __x, long double __y), + __nldbl_nexttowardf) + __attribute__ ((__const__)); +extern double __REDIRECT_NTH (nexttoward, (double __x, long double __y), + nextafter) __attribute__ ((__const__)); +extern long double __REDIRECT_NTH (nexttowardl, + (long double __x, long double __y), + nextafter) __attribute__ ((__const__)); +# endif +# endif +# endif + +# if defined __LDBL_COMPAT || defined __NO_LONG_DOUBLE_MATH + +# undef __MATHDECL_1 +# define __MATHDECL_2(type, function,suffix, args, alias) \ + extern type __REDIRECT_NTH(__MATH_PRECNAME(function,suffix), \ + args, alias) +# define __MATHDECL_1(type, function,suffix, args) \ + __MATHDECL_2(type, function,suffix, args, __CONCAT(function,suffix)) +# endif + +/* Include the file of declarations again, this time using `long double' + instead of `double' and appending l to each function name. */ + +# ifndef _Mlong_double_ +# define _Mlong_double_ long double +# endif +# define _Mdouble_ _Mlong_double_ +# ifdef __STDC__ +# define __MATH_PRECNAME(name,r) name##l##r +# else +# define __MATH_PRECNAME(name,r) name/**/l/**/r +# endif +# define _Mdouble_BEGIN_NAMESPACE __BEGIN_NAMESPACE_C99 +# define _Mdouble_END_NAMESPACE __END_NAMESPACE_C99 +# define __MATH_DECLARE_LDOUBLE 1 +# include +# undef _Mdouble_ +# undef _Mdouble_BEGIN_NAMESPACE +# undef _Mdouble_END_NAMESPACE +# undef __MATH_PRECNAME + +# endif /* __STDC__ || __GNUC__ */ + +#endif /* Use misc or ISO C99. */ +#undef __MATHDECL_1 +#undef __MATHDECL +#undef __MATHCALL + + +#if defined __USE_MISC || defined __USE_XOPEN +/* This variable is used by `gamma' and `lgamma'. */ +extern int signgam; +#endif + + +/* ISO C99 defines some generic macros which work on any data type. */ +#ifdef __USE_ISOC99 + +/* Get the architecture specific values describing the floating-point + evaluation. The following symbols will get defined: + + float_t floating-point type at least as wide as `float' used + to evaluate `float' expressions + double_t floating-point type at least as wide as `double' used + to evaluate `double' expressions + + FLT_EVAL_METHOD + Defined to + 0 if `float_t' is `float' and `double_t' is `double' + 1 if `float_t' and `double_t' are `double' + 2 if `float_t' and `double_t' are `long double' + else `float_t' and `double_t' are unspecified + + INFINITY representation of the infinity value of type `float' + + FP_FAST_FMA + FP_FAST_FMAF + FP_FAST_FMAL + If defined it indicates that the `fma' function + generally executes about as fast as a multiply and an add. + This macro is defined only iff the `fma' function is + implemented directly with a hardware multiply-add instructions. + + FP_ILOGB0 Expands to a value returned by `ilogb (0.0)'. + FP_ILOGBNAN Expands to a value returned by `ilogb (NAN)'. + + DECIMAL_DIG Number of decimal digits supported by conversion between + decimal and all internal floating-point formats. + +*/ + +/* All floating-point numbers can be put in one of these categories. */ +enum + { + FP_NAN, +# define FP_NAN FP_NAN + FP_INFINITE, +# define FP_INFINITE FP_INFINITE + FP_ZERO, +# define FP_ZERO FP_ZERO + FP_SUBNORMAL, +# define FP_SUBNORMAL FP_SUBNORMAL + FP_NORMAL +# define FP_NORMAL FP_NORMAL + }; + +/* Return number of classification appropriate for X. */ +# ifdef __NO_LONG_DOUBLE_MATH +# define fpclassify(x) \ + (sizeof (x) == sizeof (float) ? __fpclassifyf (x) : __fpclassify (x)) +# else +# define fpclassify(x) \ + (sizeof (x) == sizeof (float) \ + ? __fpclassifyf (x) \ + : sizeof (x) == sizeof (double) \ + ? __fpclassify (x) : __fpclassifyl (x)) +# endif + +/* Return nonzero value if sign of X is negative. */ +# ifdef __NO_LONG_DOUBLE_MATH +# define signbit(x) \ + (sizeof (x) == sizeof (float) ? __signbitf (x) : __signbit (x)) +# else +# define signbit(x) \ + (sizeof (x) == sizeof (float) \ + ? __signbitf (x) \ + : sizeof (x) == sizeof (double) \ + ? __signbit (x) : __signbitl (x)) +# endif + +/* Return nonzero value if X is not +-Inf or NaN. */ +# ifdef __NO_LONG_DOUBLE_MATH +# define isfinite(x) \ + (sizeof (x) == sizeof (float) ? __finitef (x) : __finite (x)) +# else +# define isfinite(x) \ + (sizeof (x) == sizeof (float) \ + ? __finitef (x) \ + : sizeof (x) == sizeof (double) \ + ? __finite (x) : __finitel (x)) +# endif + +/* Return nonzero value if X is neither zero, subnormal, Inf, nor NaN. */ +# define isnormal(x) (fpclassify (x) == FP_NORMAL) + +/* Return nonzero value if X is a NaN. We could use `fpclassify' but + we already have this functions `__isnan' and it is faster. */ +# ifdef __NO_LONG_DOUBLE_MATH +# define isnan(x) \ + (sizeof (x) == sizeof (float) ? __isnanf (x) : __isnan (x)) +# else +# define isnan(x) \ + (sizeof (x) == sizeof (float) \ + ? __isnanf (x) \ + : sizeof (x) == sizeof (double) \ + ? __isnan (x) : __isnanl (x)) +# endif + +/* Return nonzero value if X is positive or negative infinity. */ +# ifdef __NO_LONG_DOUBLE_MATH +# define isinf(x) \ + (sizeof (x) == sizeof (float) ? __isinff (x) : __isinf (x)) +# else +# define isinf(x) \ + (sizeof (x) == sizeof (float) \ + ? __isinff (x) \ + : sizeof (x) == sizeof (double) \ + ? __isinf (x) : __isinfl (x)) +# endif + +/* Bitmasks for the math_errhandling macro. */ +# define MATH_ERRNO 1 /* errno set by math functions. */ +# define MATH_ERREXCEPT 2 /* Exceptions raised by math functions. */ + +/* By default all functions support both errno and exception handling. + In gcc's fast math mode and if inline functions are defined this + might not be true. */ +# ifndef __FAST_MATH__ +# define math_errhandling (MATH_ERRNO | MATH_ERREXCEPT) +# endif + +#endif /* Use ISO C99. */ + +#ifdef __USE_MISC +/* Support for various different standard error handling behaviors. */ +typedef enum +{ + _IEEE_ = -1, /* According to IEEE 754/IEEE 854. */ + _SVID_, /* According to System V, release 4. */ + _XOPEN_, /* Nowadays also Unix98. */ + _POSIX_, + _ISOC_ /* Actually this is ISO C99. */ +} _LIB_VERSION_TYPE; + +/* This variable can be changed at run-time to any of the values above to + affect floating point error handling behavior (it may also be necessary + to change the hardware FPU exception settings). */ +extern _LIB_VERSION_TYPE _LIB_VERSION; +#endif + + +#ifdef __USE_SVID +/* In SVID error handling, `matherr' is called with this description + of the exceptional condition. + + We have a problem when using C++ since `exception' is a reserved + name in C++. */ +# ifdef __cplusplus +struct __exception +# else +struct exception +# endif + { + int type; + char *name; + double arg1; + double arg2; + double retval; + }; + +# ifdef __cplusplus +extern int matherr (struct __exception *__exc) throw (); +# else +extern int matherr (struct exception *__exc); +# endif + +# define X_TLOSS 1.41484755040568800000e+16 + +/* Types of exceptions in the `type' field. */ +# define DOMAIN 1 +# define SING 2 +# define OVERFLOW 3 +# define UNDERFLOW 4 +# define TLOSS 5 +# define PLOSS 6 + +/* SVID mode specifies returning this large value instead of infinity. */ +# define HUGE 3.40282347e+38F + +#else /* !SVID */ + +# ifdef __USE_XOPEN +/* X/Open wants another strange constant. */ +# define MAXFLOAT 3.40282347e+38F +# endif + +#endif /* SVID */ + + +/* Some useful constants. */ +#if defined __USE_BSD || defined __USE_XOPEN +# define M_E 2.7182818284590452354 /* e */ +# define M_LOG2E 1.4426950408889634074 /* log_2 e */ +# define M_LOG10E 0.43429448190325182765 /* log_10 e */ +# define M_LN2 0.69314718055994530942 /* log_e 2 */ +# define M_LN10 2.30258509299404568402 /* log_e 10 */ +# define M_PI 3.14159265358979323846 /* pi */ +# define M_PI_2 1.57079632679489661923 /* pi/2 */ +# define M_PI_4 0.78539816339744830962 /* pi/4 */ +# define M_1_PI 0.31830988618379067154 /* 1/pi */ +# define M_2_PI 0.63661977236758134308 /* 2/pi */ +# define M_2_SQRTPI 1.12837916709551257390 /* 2/sqrt(pi) */ +# define M_SQRT2 1.41421356237309504880 /* sqrt(2) */ +# define M_SQRT1_2 0.70710678118654752440 /* 1/sqrt(2) */ +#endif + +/* The above constants are not adequate for computation using `long double's. + Therefore we provide as an extension constants with similar names as a + GNU extension. Provide enough digits for the 128-bit IEEE quad. */ +#ifdef __USE_GNU +# define M_El 2.7182818284590452353602874713526625L /* e */ +# define M_LOG2El 1.4426950408889634073599246810018921L /* log_2 e */ +# define M_LOG10El 0.4342944819032518276511289189166051L /* log_10 e */ +# define M_LN2l 0.6931471805599453094172321214581766L /* log_e 2 */ +# define M_LN10l 2.3025850929940456840179914546843642L /* log_e 10 */ +# define M_PIl 3.1415926535897932384626433832795029L /* pi */ +# define M_PI_2l 1.5707963267948966192313216916397514L /* pi/2 */ +# define M_PI_4l 0.7853981633974483096156608458198757L /* pi/4 */ +# define M_1_PIl 0.3183098861837906715377675267450287L /* 1/pi */ +# define M_2_PIl 0.6366197723675813430755350534900574L /* 2/pi */ +# define M_2_SQRTPIl 1.1283791670955125738961589031215452L /* 2/sqrt(pi) */ +# define M_SQRT2l 1.4142135623730950488016887242096981L /* sqrt(2) */ +# define M_SQRT1_2l 0.7071067811865475244008443621048490L /* 1/sqrt(2) */ +#endif + + +/* When compiling in strict ISO C compatible mode we must not use the + inline functions since they, among other things, do not set the + `errno' variable correctly. */ +#if defined __STRICT_ANSI__ && !defined __NO_MATH_INLINES +# define __NO_MATH_INLINES 1 +#endif + +#if defined __USE_ISOC99 && __GNUC_PREREQ(2,97) +/* ISO C99 defines some macros to compare number while taking care for + unordered numbers. Many FPUs provide special instructions to support + these operations. Generic support in GCC for these as builtins went + in before 3.0.0, but not all cpus added their patterns. We define + versions that use the builtins here, and will + undef/redefine as appropriate for the specific GCC version in use. */ +# define isgreater(x, y) __builtin_isgreater(x, y) +# define isgreaterequal(x, y) __builtin_isgreaterequal(x, y) +# define isless(x, y) __builtin_isless(x, y) +# define islessequal(x, y) __builtin_islessequal(x, y) +# define islessgreater(x, y) __builtin_islessgreater(x, y) +# define isunordered(u, v) __builtin_isunordered(u, v) +#endif + +/* Get machine-dependent inline versions (if there are any). */ +#ifdef __USE_EXTERN_INLINES +# include +#endif + +/* Define special entry points to use when the compiler got told to + only expect finite results. */ +#if defined __FINITE_MATH_ONLY__ && __FINITE_MATH_ONLY__ > 0 +# include +#endif + +#ifdef __USE_ISOC99 +/* If we've still got undefined comparison macros, provide defaults. */ + +/* Return nonzero value if X is greater than Y. */ +# ifndef isgreater +# define isgreater(x, y) \ + (__extension__ \ + ({ __typeof__(x) __x = (x); __typeof__(y) __y = (y); \ + !isunordered (__x, __y) && __x > __y; })) +# endif + +/* Return nonzero value if X is greater than or equal to Y. */ +# ifndef isgreaterequal +# define isgreaterequal(x, y) \ + (__extension__ \ + ({ __typeof__(x) __x = (x); __typeof__(y) __y = (y); \ + !isunordered (__x, __y) && __x >= __y; })) +# endif + +/* Return nonzero value if X is less than Y. */ +# ifndef isless +# define isless(x, y) \ + (__extension__ \ + ({ __typeof__(x) __x = (x); __typeof__(y) __y = (y); \ + !isunordered (__x, __y) && __x < __y; })) +# endif + +/* Return nonzero value if X is less than or equal to Y. */ +# ifndef islessequal +# define islessequal(x, y) \ + (__extension__ \ + ({ __typeof__(x) __x = (x); __typeof__(y) __y = (y); \ + !isunordered (__x, __y) && __x <= __y; })) +# endif + +/* Return nonzero value if either X is less than Y or Y is less than X. */ +# ifndef islessgreater +# define islessgreater(x, y) \ + (__extension__ \ + ({ __typeof__(x) __x = (x); __typeof__(y) __y = (y); \ + !isunordered (__x, __y) && (__x < __y || __y < __x); })) +# endif + +/* Return nonzero value if arguments are unordered. */ +# ifndef isunordered +# define isunordered(u, v) \ + (__extension__ \ + ({ __typeof__(u) __u = (u); __typeof__(v) __v = (v); \ + fpclassify (__u) == FP_NAN || fpclassify (__v) == FP_NAN; })) +# endif + +#endif + +/* Useful constants. */ + +#define MAXFLOAT 3.40282347e+38F + +#define M_E 2.7182818284590452354 +#define M_LOG2E 1.4426950408889634074 +#define M_LOG10E 0.43429448190325182765 +#define M_LN2 _M_LN2 +#define M_LN10 2.30258509299404568402 +#define M_PI 3.14159265358979323846 +#define M_TWOPI (M_PI * 2.0) +#define M_PI_2 1.57079632679489661923 +#define M_PI_4 0.78539816339744830962 +#define M_3PI_4 2.3561944901923448370E0 +#define M_SQRTPI 1.77245385090551602792981 +#define M_1_PI 0.31830988618379067154 +#define M_2_PI 0.63661977236758134308 +#define M_2_SQRTPI 1.12837916709551257390 +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 +#define M_SQRT2 1.41421356237309504880 +#define M_SQRT1_2 0.70710678118654752440 +#define M_LN2LO 1.9082149292705877000E-10 +#define M_LN2HI 6.9314718036912381649E-1 +#define M_SQRT3 1.73205080756887719000 +#define M_IVLN10 0.43429448190325182765 /* 1 / log(10) */ +#define M_LOG2_E _M_LN2 +#define M_INVLN2 1.4426950408889633870E0 /* 1 / log(2) */ + + +#define M_E_F 2.7182818284590452354f +#define M_LOG2E_F 1.4426950408889634074f +#define M_LOG10E_F 0.43429448190325182765f +#define M_LN2_F _M_LN2_F +#define M_LN10_F 2.30258509299404568402f +#define M_PI_F 3.14159265358979323846f +#define M_TWOPI_F (M_PI_F * 2.0f) +#define M_PI_2_F 1.57079632679489661923f +#define M_PI_4_F 0.78539816339744830962f +#define M_3PI_4_F 2.3561944901923448370E0f +#define M_SQRTPI_F 1.77245385090551602792981f +#define M_1_PI_F 0.31830988618379067154f +#define M_2_PI_F 0.63661977236758134308f +#define M_2_SQRTPI_F 1.12837916709551257390f +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f +#define M_SQRT2_F 1.41421356237309504880f +#define M_SQRT1_2_F 0.70710678118654752440f +#define M_LN2LO_F 1.9082149292705877000E-10f +#define M_LN2HI_F 6.9314718036912381649E-1f +#define M_SQRT3_F 1.73205080756887719000f +#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ +#define M_LOG2_E_F _M_LN2_F +#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ + +__END_DECLS + + +#endif /* math.h */ diff --git a/nuttx/arch/sim/include/syscall.h b/nuttx/arch/sim/include/syscall.h new file mode 100644 index 000000000000..7ffcbf5e69c6 --- /dev/null +++ b/nuttx/arch/sim/include/syscall.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * arch/sim/include/syscall.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* This file should never be included directed but, rather, only indirectly + * through include/syscall.h or include/sys/sycall.h + */ + +#ifndef __ARCH_SIM_INCLUDE_SYSCALL_H +#define __ARCH_SIM_INCLUDE_SYSCALL_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Inline functions + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifndef __ASSEMBLY__ +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +#undef EXTERN +#ifdef __cplusplus +} +#endif +#endif + +#endif /* __ARCH_SIM_INCLUDE_SYSCALL_H */ + diff --git a/nuttx/arch/sim/include/types.h b/nuttx/arch/sim/include/types.h new file mode 100644 index 000000000000..719944f1f558 --- /dev/null +++ b/nuttx/arch/sim/include/types.h @@ -0,0 +1,96 @@ +/************************************************************************ + * arch/sim/include/types.h + * + * Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************/ + +/* This file should never be included directed but, rather, + * only indirectly through sys/types.h + */ + +#ifndef __ARCH_SIM_INCLUDE_TYPES_H +#define __ARCH_SIM_INCLUDE_TYPES_H + +/************************************************************************ + * Included Files + ************************************************************************/ + +/************************************************************************ + * Definitions + ************************************************************************/ + +/************************************************************************ + * Type Declarations + ************************************************************************/ + +#ifndef __ASSEMBLY__ + +/* These are the sizes of the standard integer types. NOTE that these type + * names have a leading underscore character. This file will be included + * (indirectly) by include/stdint.h and typedef'ed to the final name without + * the underscore character. This roundabout way of doings things allows + * the stdint.h to be removed from the include/ directory in the event that + * the user prefers to use the definitions provided by their toolchain header + * files + */ + +typedef signed char _int8_t; +typedef unsigned char _uint8_t; + +typedef signed short _int16_t; +typedef unsigned short _uint16_t; + +typedef signed int _int32_t; +typedef unsigned int _uint32_t; + +typedef signed long long _int64_t; +typedef unsigned long long _uint64_t; +#define __INT64_DEFINED + +/* A pointer is 4 bytes */ + +typedef signed int _intptr_t; +typedef unsigned int _uintptr_t; + +/* This is the size of the interrupt state save returned by + * irqsave() + */ + +typedef unsigned int irqstate_t; + +#endif /* __ASSEMBLY__ */ + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#endif /* __ARCH_SIM_INCLUDE_TYPES_H */ diff --git a/nuttx/arch/sim/src/GNU/Linux-names.dat b/nuttx/arch/sim/src/GNU/Linux-names.dat new file mode 100644 index 000000000000..dd4abcd3e91d --- /dev/null +++ b/nuttx/arch/sim/src/GNU/Linux-names.dat @@ -0,0 +1,36 @@ +calloc NXcalloc +close NXclose +closedir NXclosedir +dup NXdup +free NXfree +fclose NXfclose +fopen NXfopen +fputc NXfputc +fread NXfread +fwrite NXfwrite +fsync NXfsync +gettimeofday NXgettimeofday +ioctl NXioctl +lseek NXlseek +malloc NXmalloc +malloc_init NXmalloc_init +mkdir NXmkdir +mount NXmount +open NXopen +opendir NXopendir +read NXread +realloc NXrealloc +rewinddir NXrewinddir +rmdir NXrmdir +seekdir NXseekdir +select NXselect +sleep NXsleep +/*socket NXsocket*/ +stat NXstat +statfs NXstatfs +system NXsystem +umount NXumount +unlink NXunlink +usleep NXusleep +write NXwrite +zmalloc NXzmalloc diff --git a/nuttx/arch/sim/src/Makefile b/nuttx/arch/sim/src/Makefile new file mode 100644 index 000000000000..cf8008b16b9c --- /dev/null +++ b/nuttx/arch/sim/src/Makefile @@ -0,0 +1,230 @@ +############################################################################ +# arch/sim/src/Makefile +# +# Copyright (C) 2007, 2008, 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = up_setjmp.S +AOBJS = $(ASRCS:.S=$(OBJEXT)) +CSRCS = up_initialize.c up_idle.c up_interruptcontext.c \ + up_initialstate.c up_createstack.c up_usestack.c \ + up_releasestack.c up_unblocktask.c up_blocktask.c \ + up_releasepending.c up_reprioritizertr.c \ + up_exit.c up_schedulesigaction.c up_allocateheap.c \ + up_devconsole.c +HOSTSRCS = up_stdio.c up_hostusleep.c + +ifeq ($(CONFIG_NX_LCDDRIVER),y) + CSRCS += up_lcd.c +else + CSRCS += up_framebuffer.c +ifeq ($(CONFIG_SIM_X11FB),y) + HOSTSRCS += up_x11framebuffer.c +ifeq ($(CONFIG_SIM_TOUCHSCREEN),y) + CSRCS += up_touchscreen.c + HOSTSRCS += up_x11eventloop.c +endif +endif +endif + +ifeq ($(CONFIG_ELF),y) +CSRCS += up_elf.c +endif + +ifeq ($(CONFIG_FS_FAT),y) +CSRCS += up_blockdevice.c up_deviceimage.c +endif + +ifeq ($(CONFIG_ARCH_ROMGETC),y) +CSRCS += up_romgetc.c +endif + +ifeq ($(CONFIG_NET),y) +CSRCS += up_uipdriver.c +HOSTCFLAGS += -DNETDEV_BUFSIZE=$(CONFIG_NET_BUFSIZE) +ifneq ($(HOSTOS),Cygwin) +HOSTSRCS += up_tapdev.c up_netdev.c +else +HOSTSRCS += up_wpcap.c up_netdev.c +DRVLIB = /lib/w32api/libws2_32.a /lib/w32api/libiphlpapi.a +endif +endif + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +NUTTXOBJS = $(AOBJS) $(COBJS) +HOSTOBJS = $(HOSTSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) $(HOSTSRCS) +OBJS = $(AOBJS) $(COBJS) $(HOSTOBJS) + +# Determine which standard libraries will need to be linked in + +ifeq ($(CONFIG_SIM_X11FB),y) + STDLIBS += -lX11 -lXext +ifeq ($(CONFIG_SIM_TOUCHSCREEN),y) + STDLIBS += -lpthread +endif +endif + +EXTRA_LIBS ?= + +ifeq ($(CONFIG_FS_FAT),y) +STDLIBS += -lz +endif + +STDLIBS += -lc + +LIBGCC := "${shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name}" +GCC_LIBDIR := ${shell dirname $(LIBGCC)} + +# Determine which objects are required in the link. The +# up_head object normally draws in all that is needed, but +# there are a fews that must be included because they +# are called only from the host OS-specific logic (HOSTOBJS) + +LINKOBJS = up_head$(OBJEXT) +REQUIREDOBJS = $(LINKOBJS) + +ifeq ($(CONFIG_SIM_X11FB),y) +ifeq ($(CONFIG_SIM_TOUCHSCREEN),y) + REQUIREDOBJS += up_touchscreen.o +endif +endif + +# Determine which NuttX libraries will need to be linked in +# Most are provided by LINKLIBS on the MAKE command line + +LINKLIBS = +LDPATHS = $(addprefix -L$(TOPDIR)/,$(dir $(LINKLIBS))) +LDLIBS = $(patsubst lib%,-l%,$(basename $(notdir $(LINKLIBS)))) + +# Add the board-specific library and directory + +LDPATHS += -L board -L $(GCC_LIBDIR) +LDLIBS += -lboard + +# Make targets begin here + +all: up_head$(OBJEXT) libarch$(LIBEXT) + +.PHONY: board/libboard$(LIBEXT) export_head clean distclean cleanrel depend + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +$(HOSTOBJS): %$(OBJEXT): %.c + $(Q) echo "CC: $<" + $(Q) $(CC) -c $(HOSTCFLAGS) $< -o $@ + +# The architecture-specific library + +libarch$(LIBEXT): $(NUTTXOBJS) + $(Q) ( for obj in $(NUTTXOBJS) ; do \ + $(call ARCHIVE, $@, $${obj}); \ + done ; ) + +# The "board"-specific library. Of course, there really are no boards in +# the simulation. However, this is a good place to keep parts of the simulation +# that are not hardware-related. + +board/libboard$(LIBEXT): + $(Q) $(MAKE) -C board TOPDIR="$(TOPDIR)" libboard$(LIBEXT) EXTRADEFINES=$(EXTRADEFINES) + +# A partially linked object containing only NuttX code (no interface to host OS) +# Change the names of most symbols that conflict with libc symbols. + +GNU: + $(Q) mkdir ./GNU + +GNU/Linux-names.dat: GNU nuttx-names.dat + $(Q) cp nuttx-names.dat $@ + +Cygwin-names.dat: nuttx-names.dat + $(Q) cat $^ | sed -e "s/^/_/g" >$@ + +nuttx.rel : libarch$(LIBEXT) board/libboard$(LIBEXT) $(HOSTOS)-names.dat $(LINKOBJS) + $(Q) echo "LD: nuttx.rel" + $(Q) $(LD) -r $(LDLINKFLAGS) $(LDPATHS) -o $@ $(REQUIREDOBJS) --start-group $(LDLIBS) $(EXTRA_LIBS) --end-group + $(Q) $(OBJCOPY) --redefine-syms=$(HOSTOS)-names.dat $@ + +# Generate the final NuttX binary by linking the host-specific objects with the NuttX +# specific objects (with munged names) + +nuttx$(EXEEXT): cleanrel nuttx.rel $(HOSTOBJS) + $(Q) echo "LD: nuttx$(EXEEXT)" + $(Q) $(CC) $(CCLINKFLAGS) $(LDPATHS) -o $(TOPDIR)/$@ nuttx.rel $(HOSTOBJS) $(DRVLIB) $(STDLIBS) + $(Q) $(NM) $(TOPDIR)/$@ | \ + grep -v '\(compiled\)\|\(\.o$$\)\|\( [aUw] \)\|\(\.\.ng$$\)\|\(LASH[RL]DI\)' | \ + sort > $(TOPDIR)/System.map + $(Q) rm -f nuttx.rel + +# This is part of the top-level export target + +export_head: board/libboard$(LIBEXT) up_head.o $(HOSTOBJS) + cp up_head.o $(HOSTOBJS) ${EXPORT_DIR}/startup + cp nuttx-names.dat ${EXPORT_DIR}/libs + echo main NXmain >> ${EXPORT_DIR}/libs/nuttx-names.dat + +# Dependencies + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +cleanrel: + $(Q) rm -f nuttx.rel GNU/Linux-names.dat Cygwin-names.dat + +clean: cleanrel + $(Q) if [ -e board/Makefile ]; then \ + $(MAKE) -C board TOPDIR="$(TOPDIR)" clean ; \ + fi + $(Q) rm -f nuttx.rel libarch$(LIBEXT) *~ .*.swp + $(call CLEAN) + +distclean: clean + $(Q) if [ -e board/Makefile ]; then \ + $(MAKE) -C board TOPDIR="$(TOPDIR)" distclean ; \ + fi + $(Q) rm -f Make.dep .depend + $(Q) rm -rf GNU + +-include Make.dep diff --git a/nuttx/arch/sim/src/nuttx-names.dat b/nuttx/arch/sim/src/nuttx-names.dat new file mode 100644 index 000000000000..dd4abcd3e91d --- /dev/null +++ b/nuttx/arch/sim/src/nuttx-names.dat @@ -0,0 +1,36 @@ +calloc NXcalloc +close NXclose +closedir NXclosedir +dup NXdup +free NXfree +fclose NXfclose +fopen NXfopen +fputc NXfputc +fread NXfread +fwrite NXfwrite +fsync NXfsync +gettimeofday NXgettimeofday +ioctl NXioctl +lseek NXlseek +malloc NXmalloc +malloc_init NXmalloc_init +mkdir NXmkdir +mount NXmount +open NXopen +opendir NXopendir +read NXread +realloc NXrealloc +rewinddir NXrewinddir +rmdir NXrmdir +seekdir NXseekdir +select NXselect +sleep NXsleep +/*socket NXsocket*/ +stat NXstat +statfs NXstatfs +system NXsystem +umount NXumount +unlink NXunlink +usleep NXusleep +write NXwrite +zmalloc NXzmalloc diff --git a/nuttx/arch/sim/src/up_allocateheap.c b/nuttx/arch/sim/src/up_allocateheap.c new file mode 100644 index 000000000000..47400ec7c3a3 --- /dev/null +++ b/nuttx/arch/sim/src/up_allocateheap.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * up_allocateheap.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static uint8_t sim_heap[SIM_HEAP_SIZE]; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_allocate_heap + * + * Description: + * The heap may be statically allocated by + * defining CONFIG_HEAP_BASE and CONFIG_HEAP_SIZE. If these + * are not defined, then this function will be called to + * dynamically set aside the heap region. + * + ****************************************************************************/ + +void up_allocate_heap(void **heap_start, size_t *heap_size) +{ + *heap_start = sim_heap; + *heap_size = SIM_HEAP_SIZE; +} diff --git a/nuttx/arch/sim/src/up_blockdevice.c b/nuttx/arch/sim/src/up_blockdevice.c new file mode 100644 index 000000000000..edcb6f3774a4 --- /dev/null +++ b/nuttx/arch/sim/src/up_blockdevice.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * up_blockdevice.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +#define NSECTORS 2048 +#define LOGICAL_SECTOR_SIZE 512 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_registerblockdevice + * + * Description: Register the FAT ramdisk + * + ****************************************************************************/ + +void up_registerblockdevice(void) +{ + ramdisk_register(0, (uint8_t*)up_deviceimage(), NSECTORS, LOGICAL_SECTOR_SIZE, true); +} diff --git a/nuttx/arch/sim/src/up_blocktask.c b/nuttx/arch/sim/src/up_blocktask.c new file mode 100644 index 000000000000..c69c979aeb4f --- /dev/null +++ b/nuttx/arch/sim/src/up_blocktask.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * up_blocktask.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include + +#include + +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_block_task + * + * Description: + * The currently executing task at the head of + * the ready to run list must be stopped. Save its context + * and move it to the inactive list specified by task_state. + * + * Inputs: + * tcb: Refers to a task in the ready-to-run list (normally + * the task at the head of the list). It most be + * stopped, its context saved and moved into one of the + * waiting task lists. It it was the task at the head + * of the ready-to-run list, then a context to the new + * ready to run task must be performed. + * task_state: Specifies which waiting task list should be + * hold the blocked task TCB. + * + ****************************************************************************/ + +void up_block_task(_TCB *tcb, tstate_t task_state) +{ + /* Verify that the context switch can be performed */ + if ((tcb->task_state < FIRST_READY_TO_RUN_STATE) || + (tcb->task_state > LAST_READY_TO_RUN_STATE)) + { + PANIC(OSERR_BADBLOCKSTATE); + } + else + { + _TCB *rtcb = (_TCB*)g_readytorun.head; + bool switch_needed; + + sdbg("Blocking TCB=%p\n", tcb); + + /* Remove the tcb task from the ready-to-run list. If we + * are blocking the task at the head of the task list (the + * most likely case), then a context switch to the next + * ready-to-run task is needed. In this case, it should + * also be true that rtcb == tcb. + */ + + switch_needed = sched_removereadytorun(tcb); + + /* Add the task to the specified blocked task list */ + + sched_addblocked(tcb, (tstate_t)task_state); + + /* If there are any pending tasks, then add them to the g_readytorun + * task list now + */ + + if (g_pendingtasks.head) + { + switch_needed |= sched_mergepending(); + } + + /* Now, perform the context switch if one is needed */ + + if (switch_needed) + { + /* Copy the exception context into the TCB at the (old) head of the + * g_readytorun Task list. if up_setjmp returns a non-zero + * value, then this is really the previously running task restarting! + */ + + if (!up_setjmp(rtcb->xcp.regs)) + { + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + sdbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + sdbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } + } +} diff --git a/nuttx/arch/sim/src/up_createstack.c b/nuttx/arch/sim/src/up_createstack.c new file mode 100644 index 000000000000..be8fe278b41c --- /dev/null +++ b/nuttx/arch/sim/src/up_createstack.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * arch/sim/src/up_createstack.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_create_stack + * + * Description: + * Allocate a stack for a new thread and setup + * up stack-related information in the TCB. + * + * The following TCB fields must be initialized: + * adj_stack_size: Stack size after adjustment for hardware, + * processor, etc. This value is retained only for debug + * purposes. + * stack_alloc_ptr: Pointer to allocated stack + * adj_stack_ptr: Adjusted stack_alloc_ptr for HW. The + * initial value of the stack pointer. + * + * Inputs: + * tcb: The TCB of new task + * stack_size: The requested stack size. At least this much + * must be allocated. + * + ****************************************************************************/ + +int up_create_stack(_TCB *tcb, size_t stack_size) +{ + int ret = ERROR; + + /* Move up to next even word boundary if necessary */ + + size_t adj_stack_size = (stack_size + 3) & ~3; + size_t adj_stack_words = adj_stack_size >> 2; + + /* Allocate the memory for the stack */ + + uint32_t *stack_alloc_ptr = (uint32_t*)kmalloc(adj_stack_size); + if (stack_alloc_ptr) + { + /* This is the address of the last word in the allocation */ + + size_t *adj_stack_ptr = &stack_alloc_ptr[adj_stack_words - 1]; + + /* Save the values in the TCB */ + + tcb->adj_stack_size = adj_stack_size; + tcb->stack_alloc_ptr = stack_alloc_ptr; + tcb->adj_stack_ptr = adj_stack_ptr; + ret = OK; + } + return ret; +} diff --git a/nuttx/arch/sim/src/up_devconsole.c b/nuttx/arch/sim/src/up_devconsole.c new file mode 100644 index 000000000000..bd2be0f10f11 --- /dev/null +++ b/nuttx/arch/sim/src/up_devconsole.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * up_devconsole.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "up_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static ssize_t devconsole_read(struct file *, char *, size_t); +static ssize_t devconsole_write(struct file *, const char *, size_t); +#ifndef CONFIG_DISABLE_POLL +static int devconsole_poll(FAR struct file *filep, FAR struct pollfd *fds, + bool setup); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct file_operations devconsole_fops = +{ + .read = devconsole_read, + .write = devconsole_write, +#ifndef CONFIG_DISABLE_POLL + .poll = devconsole_poll, +#endif +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static ssize_t devconsole_read(struct file *filp, char *buffer, size_t len) +{ + return up_hostread(buffer, len); +} + +static ssize_t devconsole_write(struct file *filp, const char *buffer, size_t len) +{ + return up_hostwrite(buffer, len); +} + +#ifndef CONFIG_DISABLE_POLL +static int devconsole_poll(FAR struct file *filep, FAR struct pollfd *fds, + bool setup) +{ + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void up_devconsole(void) +{ + (void)register_driver("/dev/console", &devconsole_fops, 0666, NULL); +} diff --git a/nuttx/arch/sim/src/up_deviceimage.c b/nuttx/arch/sim/src/up_deviceimage.c new file mode 100644 index 000000000000..e32f64927516 --- /dev/null +++ b/nuttx/arch/sim/src/up_deviceimage.c @@ -0,0 +1,356 @@ +/**************************************************************************** + * up_deviceimage.c + * + * Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#ifdef VFAT_STANDALONE +# include +# include +# include +#else +# include +# include +# include +# include +# include "up_internal.h" +#endif + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +#ifdef VFAT_STANDALONE +# define sdbg(format, arg...) printf(format, ##arg) +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* This array holds a compressed VFAT file system created with: + * + * /sbin/mkdosfs -C -F 32 -I -n "NuttXTestVol" -S 512 -v nuttx-test.vfat 1024 + * sudo mkdir /mnt/loop + * sudo mount -o loop nuttx-test.vfat /mnt/loop + * mkdir /mnt/loop/TestDir + * echo "This is a test" >/mnt/loop/TestDir/TestFile.txt + * sudo umount /mnt/loop + * gzip nuttx-test.vfat + * xxd -g 1 nuttx-test.vfat.gz >some-file + * + * Then manually massaged from the gzip xxd output to zlib format. See + * http://www.faqs.org/rfcs/rfc1952.html. This amounts to: + * + * Remove all of the leading bytes through the null terminator of the file name + * Remove the last 8 bytes + * Add 0x08, 0x1d to the beginning. + */ + +static const unsigned char g_vfatdata[] = +{ + 0x08, 0x1d, 0xed, 0xdd, 0xcf, 0x6a, 0x13, 0x41, 0x18, 0x00, 0xf0, 0xa9, 0x16, 0x5b, 0x2a, 0x6d, + 0x73, 0x12, 0x3c, 0x39, 0x78, 0xf3, 0xb2, 0x87, 0x0a, 0x3d, 0x78, 0x32, 0xd0, 0x06, 0x0a, 0xa2, + 0xa5, 0xa6, 0xa5, 0x17, 0x95, 0x2d, 0xd9, 0x6a, 0x68, 0x4c, 0xda, 0xec, 0x8a, 0x2d, 0x78, 0xf3, + 0x01, 0xf4, 0x39, 0x8a, 0xb7, 0x7a, 0x13, 0xc5, 0x17, 0xe8, 0x5b, 0x78, 0xcb, 0xc5, 0x63, 0x4f, + 0xc6, 0x8d, 0x69, 0x8b, 0x45, 0xc4, 0x3f, 0x20, 0x51, 0xfc, 0xfd, 0xd8, 0xe1, 0xdb, 0xd9, 0x8f, + 0x81, 0x59, 0xe6, 0x63, 0x59, 0xd8, 0x85, 0xe9, 0xad, 0xbf, 0x7c, 0xbc, 0xd5, 0xe8, 0xe4, 0x9b, + 0x79, 0x08, 0xe7, 0xc6, 0x62, 0x38, 0x17, 0x42, 0x98, 0x3c, 0x0a, 0x21, 0x86, 0x9b, 0xe1, 0x44, + 0xe5, 0x38, 0x0e, 0x72, 0x63, 0xe1, 0x42, 0x38, 0xeb, 0xda, 0xf2, 0xbd, 0x3b, 0xb5, 0xdb, 0x4f, + 0x8a, 0x62, 0xbd, 0x9e, 0xe5, 0xc5, 0x5a, 0xa7, 0x56, 0xad, 0x5f, 0x9f, 0x8b, 0x31, 0xce, 0x5c, + 0x79, 0xf7, 0xf4, 0xd9, 0xab, 0xab, 0xef, 0x8b, 0x8b, 0x6b, 0xaf, 0x67, 0xde, 0x4c, 0x84, 0xc3, + 0xca, 0xfd, 0xde, 0xc7, 0xb9, 0x0f, 0x87, 0x97, 0x0e, 0x2f, 0xf7, 0x3e, 0xd5, 0x1f, 0x35, 0xf3, + 0x58, 0x1e, 0xed, 0x4e, 0x11, 0xd3, 0xb8, 0xd1, 0xe9, 0x14, 0xe9, 0x46, 0x2b, 0x8b, 0x8d, 0x66, + 0xbe, 0x95, 0xc4, 0xb8, 0xdc, 0xca, 0xd2, 0x3c, 0x8b, 0xcd, 0x76, 0x9e, 0x75, 0xcf, 0xe4, 0x37, + 0x5b, 0x9d, 0xed, 0xed, 0xbd, 0x98, 0xb6, 0x1b, 0xd3, 0x53, 0xdb, 0xdd, 0x2c, 0xcf, 0xcb, 0xd3, + 0xbd, 0xb8, 0x95, 0xed, 0xc5, 0xa2, 0x13, 0x8b, 0x6e, 0x99, 0x79, 0x98, 0x36, 0xdb, 0x31, 0x49, + 0x92, 0x38, 0x3d, 0x15, 0xf8, 0x91, 0xd5, 0xfd, 0x95, 0x95, 0xb4, 0x3a, 0xea, 0x59, 0xf0, 0x67, + 0x75, 0xbb, 0xd5, 0xf4, 0xed, 0x44, 0x08, 0xe3, 0xdf, 0x64, 0x56, 0xf7, 0x47, 0x30, 0x1d, 0x00, + 0x60, 0xc4, 0x7a, 0xde, 0xff, 0xff, 0x63, 0xde, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xfe, 0x76, 0x47, 0xfd, 0xfe, 0x6c, 0xbf, 0x6c, 0x27, 0x71, 0xd0, + 0x46, 0x3d, 0x27, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 0xd7, 0xf8, 0xfe, 0x0f, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xbe, 0xaf, 0x36, 0xee, 0x9c, 0x0c, 0xe1, 0xc6, 0xf3, + 0x83, 0xf9, 0x83, 0xf9, 0x61, 0x1c, 0xe6, 0xab, 0xf5, 0x90, 0x85, 0x3c, 0x14, 0x61, 0x21, 0xcc, + 0x86, 0x07, 0xcd, 0xd0, 0x2d, 0xaf, 0xf5, 0xbf, 0x18, 0xc6, 0xfa, 0xe2, 0xdd, 0xfa, 0xc2, 0xd2, + 0x4a, 0x2c, 0x55, 0x42, 0xe3, 0xc5, 0xf1, 0xf8, 0x41, 0x3c, 0x3f, 0xd2, 0xfb, 0xe2, 0xe7, 0x24, + 0xf1, 0x54, 0x25, 0x84, 0x9d, 0xe3, 0xf5, 0xdb, 0x39, 0x5d, 0xbf, 0x24, 0xf9, 0x5e, 0x7e, 0x38, + 0xfe, 0xb4, 0x3e, 0x6a, 0x65, 0x7d, 0xec, 0x36, 0x43, 0xab, 0xec, 0x26, 0x65, 0x77, 0xb7, 0xcc, + 0x15, 0x65, 0x1b, 0xd4, 0x47, 0x6d, 0xe9, 0xd6, 0x62, 0x7d, 0xbd, 0x1e, 0xcf, 0xd6, 0xc7, 0x78, + 0xf0, 0xa3, 0xc9, 0xe8, 0x9d, 0x6c, 0xc4, 0x9b, 0xc6, 0xa2, 0x7c, 0x08, 0xd8, 0x33, 0x17, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 0x77, 0x7d, 0x06 +}; + + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_deviceimage + * + * Description: Return an allocated buffer representing an in-memory VFAT + * file system. + * + ****************************************************************************/ + +char *up_deviceimage(void) +{ + char *pbuffer; + int bufsize = 1024*1024; + int offset = 0; + z_stream strm; + int ret; + + /* Ininitilize inflate state */ + + strm.zalloc = Z_NULL; + strm.zfree = Z_NULL; + strm.opaque = Z_NULL; + strm.avail_in = 0; + strm.next_in = Z_NULL; + ret = inflateInit(&strm); + if (ret != Z_OK) + { + sdbg("inflateInit FAILED: ret=%d msg=\"%s\"\n", ret, strm.msg ? strm.msg : "No message" ); + return NULL; + } + + /* Allocate a buffer to hold the decompressed buffer. We may have + * to reallocate this a few times to get the size right. + */ + pbuffer = (char*)malloc(bufsize); + + /* Set up the input buffer */ + + strm.avail_in = sizeof(g_vfatdata); + strm.next_in = (Bytef*)g_vfatdata; + + /* Run inflate() on input until output buffer not full */ + + do { + /* Set up to catch the next output chunk in the output buffer */ + + strm.avail_out = bufsize - offset; + strm.next_out = (Bytef*)&pbuffer[offset]; + + /* inflate */ + + ret = inflate(&strm, Z_NO_FLUSH); + + /* Handle inflate() error return values */ + + switch (ret) + { + case Z_NEED_DICT: + case Z_DATA_ERROR: + case Z_MEM_ERROR: + case Z_STREAM_ERROR: + sdbg("inflate FAILED: ret=%d msg=\"%s\"\n", ret, strm.msg ? strm.msg : "No message" ); + (void)inflateEnd(&strm); + free(pbuffer); + return NULL; + } + + /* If avail_out is zero, then inflate() returned only + * because it is out of buffer space. In this case, we + * will have to reallocate the buffer and try again. + */ + + if (strm.avail_out == 0) + { + int newbufsize = bufsize + 128*1024; + char *newbuffer = realloc(pbuffer, newbufsize); + if (!newbuffer) + { + free(pbuffer); + return NULL; + } + else + { + pbuffer = newbuffer; + offset = bufsize; + bufsize = newbufsize; + } + } + else + { + /* There are unused bytes in the buffer, reallocate to + * correct size. + */ + + int newbufsize = bufsize - strm.avail_out; + char *newbuffer = realloc(pbuffer, newbufsize); + if (!newbuffer) + { + free(pbuffer); + return NULL; + } + else + { + pbuffer = newbuffer; + bufsize = newbufsize; + } + } + } while (strm.avail_out == 0 && ret != Z_STREAM_END); + + (void)inflateEnd(&strm); + return pbuffer; +} + +/**************************************************************************** + * Name: main + * + * Description: Used to test decompression logic + * + * gcc -g -Wall -DVFAT_STANDALONE -lz -o vfat-test up_deviceimage.c + * + ****************************************************************************/ + +#ifdef VFAT_STANDALONE +int main(int argc, char **argv, char **envp) +{ + char *deviceimage; + int cmf; + int fdict; + int flg; + int check; + + cmf = g_vfatdata[0]; + printf("CMF=%02x: CM=%d CINFO=%d\n", cmf, cmf &0x0f, cmf >> 4); + + flg = g_vfatdata[1]; + fdict = (flg >> 5) & 1; + + printf("FLG=%02x: FCHECK=%d FDICT=%d FLEVEL=%d\n", flg, flg &0x1f, fdict, flg >> 6); + + /* The FCHECK value must be such that CMF and FLG, when viewed as + * a 16-bit unsigned integer stored in MSB order (CMF*256 + FLG), + * is a multiple of 31. + */ + + check = cmf*256 + flg; + if (check % 31 != 0) + { + printf("Fails check: %04x is not a multiple of 31\n", check); + } + + deviceimage = up_deviceimage(); + if (deviceimage) + { + printf("Inflate SUCCEEDED\n"); + free(deviceimage); + return 0; + } + else + { + printf("Inflate FAILED\n"); + return 1; + } +} +#endif diff --git a/nuttx/arch/sim/src/up_elf.c b/nuttx/arch/sim/src/up_elf.c new file mode 100644 index 000000000000..80443d4d5a0b --- /dev/null +++ b/nuttx/arch/sim/src/up_elf.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * arch/sim/src/up_elf.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define R_386_32 1 +#define R_386_PC32 2 + +#define ELF_BITS 32 +#define ELF_ARCH EM_386 + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: arch_checkarch + * + * Description: + * Given the ELF header in 'hdr', verify that the ELF file is appropriate + * for the current, configured architecture. Every architecture that uses + * the ELF loader must provide this function. + * + * Input Parameters: + * hdr - The ELF header read from the ELF file. + * + * Returned Value: + * True if the architecture supports this ELF file. + * + ****************************************************************************/ + +bool arch_checkarch(FAR const Elf32_Ehdr *hdr) +{ + return hdr->e_machine == EM_386 || hdr->e_machine == EM_486; +} + +/**************************************************************************** + * Name: arch_relocate and arch_relocateadd + * + * Description: + * Perform on architecture-specific ELF relocation. Every architecture + * that uses the ELF loader must provide this function. + * + * Input Parameters: + * rel - The relocation type + * sym - The ELF symbol structure containing the fully resolved value. + * addr - The address that requires the relocation. + * + * Returned Value: + * Zero (OK) if the relocation was successful. Otherwise, a negated errno + * value indicating the cause of the relocation failure. + * + ****************************************************************************/ + +int arch_relocate(FAR const Elf32_Rel *rel, FAR const Elf32_Sym *sym, + uintptr_t addr) +{ + FAR uint32_t *ptr = (FAR uint32_t *)addr; + + switch (ELF32_R_TYPE(rel->r_info)) + { + case R_386_32: + *ptr += sym->st_value; + break; + + case R_386_PC32: + *ptr += sym->st_value - (uint32_t)ptr; + break; + + default: + return -EINVAL; + } + + return OK; +} + +int arch_relocateadd(FAR const Elf32_Rela *rel, FAR const Elf32_Sym *sym, + uintptr_t addr) +{ + bdbg("Not supported\n"); + return -ENOSYS; +} + diff --git a/nuttx/arch/sim/src/up_exit.c b/nuttx/arch/sim/src/up_exit.c new file mode 100644 index 000000000000..3f82246c97e0 --- /dev/null +++ b/nuttx/arch/sim/src/up_exit.c @@ -0,0 +1,110 @@ +/**************************************************************************** + * up_exit.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: _exit + * + * Description: + * This function causes the currently executing task to cease + * to exist. This is a special case of task_delete() where the task to + * be deleted is the currently executing task. It is more complex because + * a context switch must be perform to the next ready to run task. + * + ****************************************************************************/ + +void _exit(int status) +{ + _TCB* tcb; + + sdbg("TCB=%p exitting\n", tcb); + + /* Destroy the task at the head of the ready to run list. */ + + (void)task_deletecurrent(); + + /* Now, perform the context switch to the new ready-to-run task at the + * head of the list. + */ + + tcb = (_TCB*)g_readytorun.head; + sdbg("New Active Task TCB=%p\n", tcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (tcb->xcp.sigdeliver) + { + sdbg("Delivering signals TCB=%p\n", tcb); + ((sig_deliver_t)tcb->xcp.sigdeliver)(tcb); + tcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(tcb->xcp.regs, 1); +} + diff --git a/nuttx/arch/sim/src/up_framebuffer.c b/nuttx/arch/sim/src/up_framebuffer.c new file mode 100644 index 000000000000..35329b9d88cf --- /dev/null +++ b/nuttx/arch/sim/src/up_framebuffer.c @@ -0,0 +1,399 @@ +/**************************************************************************** + * arch/sim/src/up_framebuffer.c + * + * Copyright (C) 2008-2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include "up_internal.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +#ifndef CONFIG_SIM_FBWIDTH +# define CONFIG_SIM_FBWIDTH 480 /* Framebuffer width in pixels */ +#endif + +#ifndef CONFIG_SIM_FBHEIGHT +# define CONFIG_SIM_FBHEIGHT 240 /* Framebuffer height in pixels */ +#endif + +#ifndef CONFIG_SIM_FBBPP +# define CONFIG_SIM_FBBPP 16 /* Framebuffer bytes per pixel (RGB) */ +#endif + +#undef FB_FMT +#if CONFIG_SIM_FBBPP == 1 +# define FB_FMT FB_FMT_RGB1 +#elif CONFIG_SIM_FBBPP == 4 +# define FB_FMT FB_FMT_RGB4 +#elif CONFIG_SIM_FBBPP == 8 +# define FB_FMT FB_FMT_RGB8 +#elif CONFIG_SIM_FBBPP == 16 +# define FB_FMT FB_FMT_RGB16_565 +#elif CONFIG_SIM_FBBPP == 24 +# define FB_FMT FB_FMT_RGB24 +#elif CONFIG_SIM_FBBPP == 32 +# define FB_FMT FB_FMT_RGB32 +#else +# error "Unsupported BPP" +#endif + +/* Framebuffer characteristics in bytes */ + +#define FB_WIDTH ((CONFIG_SIM_FBWIDTH * CONFIG_SIM_FBBPP + 7) / 8) +#define FB_SIZE (FB_WIDTH * CONFIG_SIM_FBHEIGHT) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + + /* Get information about the video controller configuration and the configuration + * of each color plane. + */ + +static int up_getvideoinfo(FAR struct fb_vtable_s *vtable, FAR struct fb_videoinfo_s *vinfo); +static int up_getplaneinfo(FAR struct fb_vtable_s *vtable, int planeno, FAR struct fb_planeinfo_s *pinfo); + + /* The following is provided only if the video hardware supports RGB color mapping */ + +#ifdef CONFIG_FB_CMAP +static int up_getcmap(FAR struct fb_vtable_s *vtable, FAR struct fb_cmap_s *cmap); +static int up_putcmap(FAR struct fb_vtable_s *vtable, FAR const struct fb_cmap_s *cmap); +#endif + /* The following is provided only if the video hardware supports a hardware cursor */ + +#ifdef CONFIG_FB_HWCURSOR +static int up_getcursor(FAR struct fb_vtable_s *vtable, FAR struct fb_cursorattrib_s *attrib); +static int up_setcursor(FAR struct fb_vtable_s *vtable, FAR struct fb_setcursor_s *setttings); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* The simulated framebuffer memory */ + +#ifndef CONFIG_SIM_X11FB +static uint8_t g_fb[FB_SIZE]; +#endif + +/* This structure describes the simulated video controller */ + +static const struct fb_videoinfo_s g_videoinfo = +{ + .fmt = FB_FMT, + .xres = CONFIG_SIM_FBWIDTH, + .yres = CONFIG_SIM_FBHEIGHT, + .nplanes = 1, +}; + +#ifndef CONFIG_SIM_X11FB +/* This structure describes the single, simulated color plane */ + +static const struct fb_planeinfo_s g_planeinfo = +{ + .fbmem = (FAR void *)&g_fb, + .fblen = FB_SIZE, + .stride = FB_WIDTH, + .bpp = CONFIG_SIM_FBBPP, +}; +#else +/* This structure describes the single, X11 color plane */ + +static struct fb_planeinfo_s g_planeinfo; +#endif + +/* Current cursor position */ + +#ifdef CONFIG_FB_HWCURSOR +static struct fb_cursorpos_s g_cpos; + +/* Current cursor size */ + +#ifdef CONFIG_FB_HWCURSORSIZE +static struct fb_cursorsize_s g_csize; +#endif +#endif + +/* The framebuffer object -- There is no private state information in this simple + * framebuffer simulation. + */ + +struct fb_vtable_s g_fbobject = +{ + .getvideoinfo = up_getvideoinfo, + .getplaneinfo = up_getplaneinfo, +#ifdef CONFIG_FB_CMAP + .getcmap = up_getcmap, + .putcmap = up_putcmap, +#endif +#ifdef CONFIG_FB_HWCURSOR + .getcursor = up_getcursor, + .setcursor = up_setcursor, +#endif +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_getvideoinfo + ****************************************************************************/ + +static int up_getvideoinfo(FAR struct fb_vtable_s *vtable, + FAR struct fb_videoinfo_s *vinfo) +{ + dbg("vtable=%p vinfo=%p\n", vtable, vinfo); + if (vtable && vinfo) + { + memcpy(vinfo, &g_videoinfo, sizeof(struct fb_videoinfo_s)); + return OK; + } + dbg("Returning EINVAL\n"); + return -EINVAL; +} + +/**************************************************************************** + * Name: up_getplaneinfo + ****************************************************************************/ + +static int up_getplaneinfo(FAR struct fb_vtable_s *vtable, int planeno, + FAR struct fb_planeinfo_s *pinfo) +{ + dbg("vtable=%p planeno=%d pinfo=%p\n", vtable, planeno, pinfo); + if (vtable && planeno == 0 && pinfo) + { + memcpy(pinfo, &g_planeinfo, sizeof(struct fb_planeinfo_s)); + return OK; + } + dbg("Returning EINVAL\n"); + return -EINVAL; +} + +/**************************************************************************** + * Name: up_getcmap + ****************************************************************************/ + +#ifdef CONFIG_FB_CMAP +static int up_getcmap(FAR struct fb_vtable_s *vtable, FAR struct fb_cmap_s *cmap) +{ + int len; + int i; + + dbg("vtable=%p cmap=%p len=%d\n", vtable, cmap, cmap->len); + if (vtable && cmap) + { + for (i = cmap->first, len = 0; i < 256 && len < cmap->len; i++, len++) + { + cmap->red[i] = i; + cmap->green[i] = i; + cmap->blue[i] = i; +#ifdef CONFIG_FB_TRANSPARENCY + cmap->transp[i] = i; +#endif + } + + cmap->len = len; + return OK; + } + dbg("Returning EINVAL\n"); + return -EINVAL; +} +#endif + +/**************************************************************************** + * Name: up_putcmap + ****************************************************************************/ + +#ifdef CONFIG_FB_CMAP +static int up_putcmap(FAR struct fb_vtable_s *vtable, FAR const struct fb_cmap_s *cmap) +{ +#ifdef CONFIG_SIM_X11FB + return up_x11cmap(cmap->first, cmap->len, cmap->red, cmap->green, cmap->blue, NULL); +#else + dbg("vtable=%p cmap=%p len=%d\n", vtable, cmap, cmap->len); + if (vtable && cmap) + { + return OK; + } + dbg("Returning EINVAL\n"); + return -EINVAL; +#endif +} +#endif + +/**************************************************************************** + * Name: up_getcursor + ****************************************************************************/ + +#ifdef CONFIG_FB_HWCURSOR +static int up_getcursor(FAR struct fb_vtable_s *vtable, + FAR struct fb_cursorattrib_s *attrib) +{ + dbg("vtable=%p attrib=%p\n", vtable, attrib); + if (vtable && attrib) + { +#ifdef CONFIG_FB_HWCURSORIMAGE + attrib->fmt = FB_FMT; +#endif + dbg("pos: (x=%d, y=%d)\n", g_cpos.x, g_cpos.y); + attrib->pos = g_cpos; +#ifdef CONFIG_FB_HWCURSORSIZE + attrib->mxsize.h = CONFIG_SIM_FBHEIGHT; + attrib->mxsize.w = CONFIG_SIM_FBWIDTH; + dbg("size: (h=%d, w=%d)\n", g_csize.h, g_csize.w); + attrib->size = g_csize; +#endif + return OK; + } + dbg("Returning EINVAL\n"); + return -EINVAL; +} +#endif + +/**************************************************************************** + * Name: + ****************************************************************************/ + +#ifdef CONFIG_FB_HWCURSOR +static int up_setcursor(FAR struct fb_vtable_s *vtable, + FAR struct fb_setcursor_s *setttings) +{ + dbg("vtable=%p setttings=%p\n", vtable, setttings); + if (vtable && setttings) + { + dbg("flags: %02x\n", settings->flags); + if ((flags & FB_CUR_SETPOSITION) != 0) + { + g_cpos = settings->pos; + dbg("pos: (h:%d, w:%d)\n", g_cpos.x, g_cpos.y); + } +#ifdef CONFIG_FB_HWCURSORSIZE + if ((flags & FB_CUR_SETSIZE) != 0) + { + g_csize = settings->size; + dbg("size: (h:%d, w:%d)\n", g_csize.h, g_csize.w); + } +#endif +#ifdef CONFIG_FB_HWCURSORIMAGE + if ((flags & FB_CUR_SETIMAGE) != 0) + { + dbg("image: (h:%d, w:%d) @ %p\n", + settings->img.height, settings->img.width, settings->img.image); + } +#endif + return OK; + } + dbg("Returning EINVAL\n"); + return -EINVAL; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_fbinitialize + * + * Description: + * Initialize the video hardware + * + ****************************************************************************/ + +int up_fbinitialize(void) +{ +#ifdef CONFIG_SIM_X11FB + return up_x11initialize(CONFIG_SIM_FBWIDTH, CONFIG_SIM_FBHEIGHT, + &g_planeinfo.fbmem, &g_planeinfo.fblen, + &g_planeinfo.bpp, &g_planeinfo.stride); +#else + return OK; +#endif +} + +/**************************************************************************** + * Name: up_fbgetvplane + * + * Description: + * Return a a reference to the framebuffer object for the specified video plane. + * + * Input parameters: + * None + * + * Returned value: + * Reference to the framebuffer object (NULL on failure) + * + ***************************************************************************/ + +FAR struct fb_vtable_s *up_fbgetvplane(int vplane) +{ + if (vplane == 0) + { + return &g_fbobject; + } + else + { + return NULL; + } +} + +/**************************************************************************** + * Name: up_fbteardown + ****************************************************************************/ + +void fb_uninitialize(void) +{ +} + diff --git a/nuttx/arch/sim/src/up_head.c b/nuttx/arch/sim/src/up_head.c new file mode 100644 index 000000000000..7326e73dca43 --- /dev/null +++ b/nuttx/arch/sim/src/up_head.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * up_head.c + * + * Copyright (C) 2007-2009, 2011-2112 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static jmp_buf sim_abort; + +/**************************************************************************** + * Global Functions + ****************************************************************************/ + +int main(int argc, char **argv, char **envp) +{ + /* Power management should be initialized early in the (simulated) boot + * sequence. + */ + +#ifdef CONFIG_PM + pm_initialize(); +#endif + + /* Then start NuttX */ + + if (setjmp(sim_abort) == 0) + { + os_start(); + } + return 0; +} + +void up_assert(const uint8_t *filename, int line) +{ + fprintf(stderr, "Assertion failed at file:%s line: %d\n", filename, line); + longjmp(sim_abort, 1); +} + +void up_assert_code(const uint8_t *filename, int line, int code) +{ + fprintf(stderr, "Assertion failed at file:%s line: %d error code: %d\n", filename, line, code); + longjmp(sim_abort, 1); +} diff --git a/nuttx/arch/sim/src/up_hostusleep.c b/nuttx/arch/sim/src/up_hostusleep.c new file mode 100644 index 000000000000..df65d7be5c7d --- /dev/null +++ b/nuttx/arch/sim/src/up_hostusleep.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * arch/sim/src/up_hostusleep.c + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_hostusleep + ****************************************************************************/ + +int up_hostusleep(unsigned int usec) +{ + return usleep(usec); +} + diff --git a/nuttx/arch/sim/src/up_idle.c b/nuttx/arch/sim/src/up_idle.c new file mode 100644 index 000000000000..f493a2dbdc66 --- /dev/null +++ b/nuttx/arch/sim/src/up_idle.c @@ -0,0 +1,159 @@ +/**************************************************************************** + * up_idle.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include + +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifdef CONFIG_SIM_X11FB +static int g_x11refresh = 0; +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#if defined(CONFIG_SIM_WALLTIME) || defined(CONFIG_SIM_X11FB) +extern int up_hostusleep(unsigned int usec); +#ifdef CONFIG_SIM_X11FB +extern void up_x11update(void); +#endif +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_idle + * + * Description: + * up_idle() is the logic that will be executed when their + * is no other ready-to-run task. This is processor idle + * time and will continue until some interrupt occurs to + * cause a context switch from the idle task. + * + * Processing in this state may be processor-specific. e.g., + * this is where power management operations might be + * performed. + * + ****************************************************************************/ + +void up_idle(void) +{ + /* If the system is idle, then process "fake" timer interrupts. + * Hopefully, something will wake up. + */ + + sched_process_timer(); + + /* Run the network if enabled */ + +#ifdef CONFIG_NET + uipdriver_loop(); +#endif + + /* Fake some power management stuff for testing purposes */ + +#ifdef CONFIG_PM + { + static enum pm_state_e state = PM_NORMAL; + enum pm_state_e newstate; + + newstate = pm_checkstate(); + if (newstate != state) + { + if (pm_changestate(newstate) == OK) + { + state = newstate; + } + } + } +#endif + + /* Wait a bit so that the sched_process_timer() is called close to the + * correct rate. + */ + +#if defined(CONFIG_SIM_WALLTIME) || defined(CONFIG_SIM_X11FB) + (void)up_hostusleep(1000000 / CLK_TCK); + + /* Handle X11-related events */ + +#ifdef CONFIG_SIM_X11FB + if (g_x11initialized) + { + /* Drive the X11 event loop */ + +#ifdef CONFIG_SIM_TOUCHSCREEN + if (g_eventloop) + { + up_x11events(); + } +#endif + + /* Update the display periodically */ + + g_x11refresh += 1000000 / CLK_TCK; + if (g_x11refresh > 500000) + { + up_x11update(); + } + } +#endif +#endif +} + diff --git a/nuttx/arch/sim/src/up_initialize.c b/nuttx/arch/sim/src/up_initialize.c new file mode 100644 index 000000000000..de11c7947248 --- /dev/null +++ b/nuttx/arch/sim/src/up_initialize.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * up_initialize.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include + +#include +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_initialize + * + * Description: + * up_initialize will be called once during OS + * initialization after the basic OS services have been + * initialized. The architecture specific details of + * initializing the OS will be handled here. Such things as + * setting up interrupt service routines, starting the + * clock, and registering device drivers are some of the + * things that are different for each processor and hardware + * platform. + * + * up_initialize is called after the OS initialized but + * before the init process has been started and before the + * libraries have been initialized. OS services and driver + * services are available. + * + ****************************************************************************/ + +void up_initialize(void) +{ + /* The real purpose of the following is to make sure that lib_rawprintf + * is drawn into the link. It is needed by up_tapdev which is linked + * separately. + */ + +#ifdef CONFIG_NET + lib_rawprintf("SIM: Initializing"); +#endif + + /* Register devices */ + + devnull_register(); /* Standard /dev/null */ + devzero_register(); /* Standard /dev/zero */ + + /* Register a console (or not) */ + +#if defined(USE_DEVCONSOLE) + up_devconsole(); /* Our private /dev/console */ +#elif defined(CONFIG_RAMLOG_CONSOLE) + ramlog_consoleinit(); +#endif + +#ifdef CONFIG_SYSLOG_CHAR + syslog_initialize(); +#endif +#ifdef CONFIG_RAMLOG_SYSLOG + ramlog_sysloginit(); /* System logging device */ +#endif + +#if defined(CONFIG_FS_FAT) && !defined(CONFIG_DISABLE_MOUNTPOINT) + up_registerblockdevice(); /* Our FAT ramdisk at /dev/ram0 */ +#endif + +#ifdef CONFIG_NET + uipdriver_init(); /* Our "real" network driver */ +#endif +} diff --git a/nuttx/arch/sim/src/up_initialstate.c b/nuttx/arch/sim/src/up_initialstate.c new file mode 100644 index 000000000000..6f241fc47da7 --- /dev/null +++ b/nuttx/arch/sim/src/up_initialstate.c @@ -0,0 +1,84 @@ +/**************************************************************************** + * up_initialstate.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_initial_state + * + * Description: + * A new thread is being started and a new TCB + * has been created. This function is called to initialize + * the processor specific portions of the new TCB. + * + * This function must setup the intial architecture registers + * and/or stack so that execution will begin at tcb->start + * on the next context switch. + * + ****************************************************************************/ + +void up_initial_state(_TCB *tcb) +{ + memset(&tcb->xcp, 0, sizeof(struct xcptcontext)); + tcb->xcp.regs[JB_SP] = (uint32_t)tcb->adj_stack_ptr; + tcb->xcp.regs[JB_PC] = (uint32_t)tcb->start; +} diff --git a/nuttx/arch/sim/src/up_internal.h b/nuttx/arch/sim/src/up_internal.h new file mode 100644 index 000000000000..dff51b9cf726 --- /dev/null +++ b/nuttx/arch/sim/src/up_internal.h @@ -0,0 +1,234 @@ +/************************************************************************** + * up_internal.h + * + * Copyright (C) 2007, 2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + **************************************************************************/ + +#ifndef __ARCH_UP_INTERNAL_H +#define __ARCH_UP_INTERNAL_H + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include +#include +#include +#include + +/************************************************************************** + * Pre-processor Definitions + **************************************************************************/ +/* Configuration **********************************************************/ + +#ifndef CONFIG_SIM_X11FB +# ifdef CONFIG_SIM_TOUCHSCREEN +# error "CONFIG_SIM_TOUCHSCREEN depends on CONFIG_SIM_X11FB" +# undef CONFIG_SIM_TOUCHSCREEN +# endif +#endif + +#ifndef CONFIG_INPUT +# ifdef CONFIG_SIM_TOUCHSCREEN +# error "CONFIG_SIM_TOUCHSCREEN depends on CONFIG_INPUT" +# undef CONFIG_SIM_TOUCHSCREEN +# endif +#endif + +/* Determine which (if any) console driver to use */ + +#if !defined(CONFIG_DEV_CONSOLE) || CONFIG_NFILE_DESCRIPTORS == 0 +# undef USE_DEVCONSOLE +# undef CONFIG_RAMLOG_CONSOLE +#else +# if defined(CONFIG_RAMLOG_CONSOLE) +# undef USE_DEVCONSOLE +# else +# define USE_DEVCONSOLE 1 +# endif +#endif + +/* Determine which device to use as the system logging device */ + +#ifndef CONFIG_SYSLOG +# undef CONFIG_SYSLOG_CHAR +# undef CONFIG_RAMLOG_SYSLOG +#endif + +/* Context Switching Definitions ******************************************/ +/* Storage order: %ebx, $esi, %edi, %ebp, sp, and return PC */ + +#ifdef __ASSEMBLY__ +# define JB_EBX (0*4) +# define JB_ESI (1*4) +# define JB_EDI (2*4) +# define JB_EBP (3*4) +# define JB_SP (4*4) +# define JB_PC (5*4) +#else +# define JB_EBX (0) +# define JB_ESI (1) +# define JB_EDI (2) +# define JB_EBP (3) +# define JB_SP (4) +# define JB_PC (5) +#endif /* __ASSEMBLY__ */ + +/* Simulated Heap Definitions **********************************************/ +/* Size of the simulated heap */ + +#if CONFIG_MM_SMALL +# define SIM_HEAP_SIZE (64*1024) +#else +# define SIM_HEAP_SIZE (4*1024*1024) +#endif + +/* File System Definitions **************************************************/ +/* These definitions characterize the compressed filesystem image */ + +#define BLOCK_COUNT 1024 +#define SECTOR_OF_BACKUPT 6 +#define NUMBER_OF_FATS 2 +#define FAT_SIZE 32 +#define NUM_HIDDEN_SECTORS 0 +#define VOLUME_NAME "NuttXTestVol" +#define USE_WHOLE_DEVICE 1 +#define ROOT_DIR_ENTRIES 512 +#define RESERVED_SECTORS 32 +#define SECTORS_PER_CLUSTER 4 +#define LOGICAL_SECTOR_SIZE 512 + +/************************************************************************** + * Public Types + **************************************************************************/ + +/************************************************************************** + * Public Variables + **************************************************************************/ + +#ifndef __ASSEMBLY__ + +#ifdef CONFIG_SIM_X11FB +extern int g_x11initialized; +#ifdef CONFIG_SIM_TOUCHSCREEN +extern volatile int g_eventloop; +#endif +#endif + +/************************************************************************** + * Public Function Prototypes + **************************************************************************/ + +/* up_setjmp.S ************************************************************/ + +extern int up_setjmp(int *jb); +extern void up_longjmp(int *jb, int val) noreturn_function; + +/* up_devconsole.c ********************************************************/ + +extern void up_devconsole(void); +extern void up_registerblockdevice(void); + +/* up_deviceimage.c *******************************************************/ + +extern char *up_deviceimage(void); + +/* up_stdio.c *************************************************************/ + +extern size_t up_hostread(void *buffer, size_t len); +extern size_t up_hostwrite(const void *buffer, size_t len); + +/* up_netdev.c ************************************************************/ + +#ifdef CONFIG_NET +extern unsigned long up_getwalltime( void ); +#endif + +/* up_x11framebuffer.c ******************************************************/ + +#ifdef CONFIG_SIM_X11FB +extern int up_x11initialize(unsigned short width, unsigned short height, + void **fbmem, unsigned int *fblen, unsigned char *bpp, + unsigned short *stride); +#ifdef CONFIG_FB_CMAP +extern int up_x11cmap(unsigned short first, unsigned short len, + unsigned char *red, unsigned char *green, + unsigned char *blue, unsigned char *transp); +#endif +#endif + +/* up_eventloop.c ***********************************************************/ + +#if defined(CONFIG_SIM_X11FB) && defined(CONFIG_SIM_TOUCHSCREEN) +extern void up_x11events(void); +#endif + +/* up_eventloop.c ***********************************************************/ + +#if defined(CONFIG_SIM_X11FB) && defined(CONFIG_SIM_TOUCHSCREEN) +extern int up_buttonevent(int x, int y, int buttons); +#endif + +/* up_tapdev.c ************************************************************/ + +#if defined(CONFIG_NET) && !defined(__CYGWIN__) +extern void tapdev_init(void); +extern unsigned int tapdev_read(unsigned char *buf, unsigned int buflen); +extern void tapdev_send(unsigned char *buf, unsigned int buflen); + +#define netdev_init() tapdev_init() +#define netdev_read(buf,buflen) tapdev_read(buf,buflen) +#define netdev_send(buf,buflen) tapdev_send(buf,buflen) +#endif + +/* up_wpcap.c *************************************************************/ + +#if defined(CONFIG_NET) && defined(__CYGWIN__) +extern void wpcap_init(void); +extern unsigned int wpcap_read(unsigned char *buf, unsigned int buflen); +extern void wpcap_send(unsigned char *buf, unsigned int buflen); + +#define netdev_init() wpcap_init() +#define netdev_read(buf,buflen) wpcap_read(buf,buflen) +#define netdev_send(buf,buflen) wpcap_send(buf,buflen) +#endif + +/* up_uipdriver.c *********************************************************/ + +#ifdef CONFIG_NET +extern int uipdriver_init(void); +extern int uipdriver_setmacaddr(unsigned char *macaddr); +extern void uipdriver_loop(void); +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_UP_INTERNAL_H */ diff --git a/nuttx/arch/sim/src/up_interruptcontext.c b/nuttx/arch/sim/src/up_interruptcontext.c new file mode 100644 index 000000000000..83a17d3b6dbb --- /dev/null +++ b/nuttx/arch/sim/src/up_interruptcontext.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * up_interruptcontext.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_interrupt_context + * + * Description: + * Return true is we are currently executing in the interrupt handler + * context. + * + ****************************************************************************/ + +bool up_interrupt_context(void) +{ + /* The simulation is never in the interrupt state */ + return false; +} + diff --git a/nuttx/arch/sim/src/up_lcd.c b/nuttx/arch/sim/src/up_lcd.c new file mode 100644 index 000000000000..4c2031dd29ec --- /dev/null +++ b/nuttx/arch/sim/src/up_lcd.c @@ -0,0 +1,428 @@ +/**************************************************************************** + * arch/sim/src/up_lcd.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ +/* Check contrast selection */ + +#if !defined(CONFIG_LCD_MAXCONTRAST) +# define CONFIG_LCD_MAXCONTRAST 100 +#endif + +/* Check power setting */ + +#if !defined(CONFIG_LCD_MAXPOWER) +# define CONFIG_LCD_MAXPOWER 100 +#endif + +/* Simulated LCD geometry and color format */ + +#ifndef CONFIG_SIM_FBWIDTH +# define CONFIG_SIM_FBWIDTH 320 /* Framebuffer width in pixels */ +#endif + +#ifndef CONFIG_SIM_FBHEIGHT +# define CONFIG_SIM_FBHEIGHT 240 /* Framebuffer height in pixels */ +#endif + +#ifndef CONFIG_SIM_FBBPP +# define CONFIG_SIM_FBBPP 16 /* Framebuffer bytes per pixel (RGB) */ +#endif + +#define FB_STRIDE ((CONFIG_SIM_FBBPP * CONFIG_SIM_FBWIDTH + 7) >> 3) + +#undef FB_FMT +#if CONFIG_SIM_FBBPP == 1 +# define FB_FMT FB_FMT_RGB1 +#elif CONFIG_SIM_FBBPP == 4 +# define FB_FMT FB_FMT_RGB4 +#elif CONFIG_SIM_FBBPP == 8 +# define FB_FMT FB_FMT_RGB8 +#elif CONFIG_SIM_FBBPP == 16 +# define FB_FMT FB_FMT_RGB16_565 +#elif CONFIG_SIM_FBBPP == 24 +# define FB_FMT FB_FMT_RGB24 +#elif CONFIG_SIM_FBBPP == 32 +# define FB_FMT FB_FMT_RGB32 +#else +# error "Unsupported BPP" +#endif + +/* Debug ********************************************************************/ +/* Define CONFIG_DEBUG_LCD to enable detailed LCD debug output. Verbose debug must + * also be enabled. + */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_VERBOSE +# undef CONFIG_DEBUG_GRAPHICS +# undef CONFIG_DEBUG_LCD +#endif + +#ifndef CONFIG_DEBUG_VERBOSE +# undef CONFIG_DEBUG_LCD +#endif + +#ifdef CONFIG_DEBUG_LCD +# define lcddbg(format, arg...) vdbg(format, ##arg) +#else +# define lcddbg(x...) +#endif + +/**************************************************************************** + * Private Type Definition + ****************************************************************************/ + +/* This structure describes the state of this driver */ + +struct sim_dev_s +{ + /* Publically visible device structure */ + + struct lcd_dev_s dev; + + /* Private LCD-specific information follows */ + + uint8_t power; /* Current power setting */ +}; + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +/* LCD Data Transfer Methods */ + +static int sim_putrun(fb_coord_t row, fb_coord_t col, FAR const uint8_t *buffer, + size_t npixels); +static int sim_getrun(fb_coord_t row, fb_coord_t col, FAR uint8_t *buffer, + size_t npixels); + +/* LCD Configuration */ + +static int sim_getvideoinfo(FAR struct lcd_dev_s *dev, + FAR struct fb_videoinfo_s *vinfo); +static int sim_getplaneinfo(FAR struct lcd_dev_s *dev, unsigned int planeno, + FAR struct lcd_planeinfo_s *pinfo); + +/* LCD RGB Mapping */ + +#ifdef CONFIG_FB_CMAP +# error "RGB color mapping not supported by this driver" +#endif + +/* Cursor Controls */ + +#ifdef CONFIG_FB_HWCURSOR +# error "Cursor control not supported by this driver" +#endif + +/* LCD Specific Controls */ + +static int sim_getpower(struct lcd_dev_s *dev); +static int sim_setpower(struct lcd_dev_s *dev, int power); +static int sim_getcontrast(struct lcd_dev_s *dev); +static int sim_setcontrast(struct lcd_dev_s *dev, unsigned int contrast); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* This is working memory allocated by the LCD driver for each LCD device + * and for each color plane. This memory will hold one raster line of data. + * The size of the allocated run buffer must therefore be at least + * (bpp * xres / 8). Actual alignment of the buffer must conform to the + * bitwidth of the underlying pixel type. + * + * If there are multiple planes, they may share the same working buffer + * because different planes will not be operate on concurrently. However, + * if there are multiple LCD devices, they must each have unique run buffers. + */ + +static uint8_t g_runbuffer[FB_STRIDE]; + +/* This structure describes the overall LCD video controller */ + +static const struct fb_videoinfo_s g_videoinfo = +{ + .fmt = FB_FMT, /* Color format: RGB16-565: RRRR RGGG GGGB BBBB */ + .xres = CONFIG_SIM_FBWIDTH, /* Horizontal resolution in pixel columns */ + .yres = CONFIG_SIM_FBHEIGHT, /* Vertical resolution in pixel rows */ + .nplanes = 1, /* Number of color planes supported */ +}; + +/* This is the standard, NuttX Plane information object */ + +static const struct lcd_planeinfo_s g_planeinfo = +{ + .putrun = sim_putrun, /* Put a run into LCD memory */ + .getrun = sim_getrun, /* Get a run from LCD memory */ + .buffer = (uint8_t*)g_runbuffer, /* Run scratch buffer */ + .bpp = CONFIG_SIM_FBBPP, /* Bits-per-pixel */ +}; + +/* This is the standard, NuttX LCD driver object */ + +static struct sim_dev_s g_lcddev = +{ + .dev = + { + /* LCD Configuration */ + + .getvideoinfo = sim_getvideoinfo, + .getplaneinfo = sim_getplaneinfo, + + /* LCD RGB Mapping -- Not supported */ + /* Cursor Controls -- Not supported */ + + /* LCD Specific Controls */ + + .getpower = sim_getpower, + .setpower = sim_setpower, + .getcontrast = sim_getcontrast, + .setcontrast = sim_setcontrast, + }, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: sim_putrun + * + * Description: + * This method can be used to write a partial raster line to the LCD: + * + * row - Starting row to write to (range: 0 <= row < yres) + * col - Starting column to write to (range: 0 <= col <= xres-npixels) + * buffer - The buffer containing the run to be written to the LCD + * npixels - The number of pixels to write to the LCD + * (range: 0 < npixels <= xres-col) + * + ****************************************************************************/ + +static int sim_putrun(fb_coord_t row, fb_coord_t col, FAR const uint8_t *buffer, + size_t npixels) +{ + lcddbg("row: %d col: %d npixels: %d\n", row, col, npixels); + return OK; +} + +/**************************************************************************** + * Name: sim_getrun + * + * Description: + * This method can be used to read a partial raster line from the LCD: + * + * row - Starting row to read from (range: 0 <= row < yres) + * col - Starting column to read read (range: 0 <= col <= xres-npixels) + * buffer - The buffer in which to return the run read from the LCD + * npixels - The number of pixels to read from the LCD + * (range: 0 < npixels <= xres-col) + * + ****************************************************************************/ + +static int sim_getrun(fb_coord_t row, fb_coord_t col, FAR uint8_t *buffer, + size_t npixels) +{ + lcddbg("row: %d col: %d npixels: %d\n", row, col, npixels); + return -ENOSYS; +} + +/**************************************************************************** + * Name: sim_getvideoinfo + * + * Description: + * Get information about the LCD video controller configuration. + * + ****************************************************************************/ + +static int sim_getvideoinfo(FAR struct lcd_dev_s *dev, + FAR struct fb_videoinfo_s *vinfo) +{ + DEBUGASSERT(dev && vinfo); + gvdbg("fmt: %d xres: %d yres: %d nplanes: %d\n", + g_videoinfo.fmt, g_videoinfo.xres, g_videoinfo.yres, g_videoinfo.nplanes); + memcpy(vinfo, &g_videoinfo, sizeof(struct fb_videoinfo_s)); + return OK; +} + +/**************************************************************************** + * Name: sim_getplaneinfo + * + * Description: + * Get information about the configuration of each LCD color plane. + * + ****************************************************************************/ + +static int sim_getplaneinfo(FAR struct lcd_dev_s *dev, unsigned int planeno, + FAR struct lcd_planeinfo_s *pinfo) +{ + DEBUGASSERT(dev && pinfo && planeno == 0); + gvdbg("planeno: %d bpp: %d\n", planeno, g_planeinfo.bpp); + memcpy(pinfo, &g_planeinfo, sizeof(struct lcd_planeinfo_s)); + return OK; +} + +/**************************************************************************** + * Name: sim_getpower + * + * Description: + * Get the LCD panel power status (0: full off - CONFIG_LCD_MAXPOWER: + * full on). On backlit LCDs, this setting may correspond to the backlight setting. + * + ****************************************************************************/ + +static int sim_getpower(struct lcd_dev_s *dev) +{ + gvdbg("power: %d\n", 0); + return g_lcddev.power; +} + +/**************************************************************************** + * Name: sim_setpower + * + * Description: + * Enable/disable LCD panel power (0: full off - CONFIG_LCD_MAXPOWER: + * full on). On backlit LCDs, this setting may correspond to the backlight + * setting. + * + ****************************************************************************/ + +static int sim_setpower(struct lcd_dev_s *dev, int power) +{ + gvdbg("power: %d\n", power); + DEBUGASSERT(power <= CONFIG_LCD_MAXPOWER); + + /* Set new power level */ + + g_lcddev.power = power; + return OK; +} + +/**************************************************************************** + * Name: sim_getcontrast + * + * Description: + * Get the current contrast setting (0-CONFIG_LCD_MAXCONTRAST). + * + ****************************************************************************/ + +static int sim_getcontrast(struct lcd_dev_s *dev) +{ + gvdbg("Not implemented\n"); + return -ENOSYS; +} + +/**************************************************************************** + * Name: sim_setcontrast + * + * Description: + * Set LCD panel contrast (0-CONFIG_LCD_MAXCONTRAST). + * + ****************************************************************************/ + +static int sim_setcontrast(struct lcd_dev_s *dev, unsigned int contrast) +{ + gvdbg("contrast: %d\n", contrast); + return -ENOSYS; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_lcdinitialize + * + * Description: + * Initialize the LCD video hardware. The initial state of the LCD is + * fully initialized, display memory cleared, and the LCD ready to use, + * but with the power setting at 0 (full off). + * + ****************************************************************************/ + +int up_lcdinitialize(void) +{ + gvdbg("Initializing\n"); + return OK; +} + +/**************************************************************************** + * Name: up_lcdgetdev + * + * Description: + * Return a a reference to the LCD object for the specified LCD. This + * allows support for multiple LCD devices. + * + ****************************************************************************/ + +FAR struct lcd_dev_s *up_lcdgetdev(int lcddev) +{ + DEBUGASSERT(lcddev == 0); + return &g_lcddev.dev; +} + +/**************************************************************************** + * Name: up_lcduninitialize + * + * Description: + * Unitialize the LCD support + * + ****************************************************************************/ + +void up_lcduninitialize(void) +{ +} + diff --git a/nuttx/arch/sim/src/up_netdev.c b/nuttx/arch/sim/src/up_netdev.c new file mode 100644 index 000000000000..e02b30ef6d15 --- /dev/null +++ b/nuttx/arch/sim/src/up_netdev.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * arch/sim/src/up_tapdev.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +#ifndef NULL +# define NULL (void*)0 +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +unsigned long up_getwalltime( void ) +{ + struct timeval tm; + (void)gettimeofday(&tm, NULL); + return tm.tv_sec*1000 + tm.tv_usec/1000; +} diff --git a/nuttx/arch/sim/src/up_releasepending.c b/nuttx/arch/sim/src/up_releasepending.c new file mode 100644 index 000000000000..52d76bcace85 --- /dev/null +++ b/nuttx/arch/sim/src/up_releasepending.c @@ -0,0 +1,119 @@ +/**************************************************************************** + * up_releasepending.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_release_pending + * + * Description: + * Release and ready-to-run tasks that have + * collected in the pending task list. This can call a + * context switch if a new task is placed at the head of + * the ready to run list. + * + ****************************************************************************/ + +void up_release_pending(void) +{ + _TCB *rtcb = (_TCB*)g_readytorun.head; + + sdbg("From TCB=%p\n", rtcb); + + /* Merge the g_pendingtasks list into the g_readytorun task list */ + + /* sched_lock(); */ + if (sched_mergepending()) + { + /* The currently active task has changed! Copy the exception context + * into the TCB of the task that was currently active. if up_setjmp + * returns a non-zero value, then this is really the previously + * running task restarting! + */ + + if (!up_setjmp(rtcb->xcp.regs)) + { + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + sdbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + sdbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } +} diff --git a/nuttx/arch/sim/src/up_releasestack.c b/nuttx/arch/sim/src/up_releasestack.c new file mode 100644 index 000000000000..ef09fcb2a9a1 --- /dev/null +++ b/nuttx/arch/sim/src/up_releasestack.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * up_releasestack.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_release_stack + * + * Description: + * A task has been stopped. Free all stack + * related resources retained int the defunct TCB. + * + ****************************************************************************/ + +void up_release_stack(_TCB *dtcb) +{ + if (dtcb->stack_alloc_ptr) + { + free(dtcb->stack_alloc_ptr); + } + + dtcb->stack_alloc_ptr = NULL; + dtcb->adj_stack_size = 0; + dtcb->adj_stack_ptr = NULL; +} diff --git a/nuttx/arch/sim/src/up_reprioritizertr.c b/nuttx/arch/sim/src/up_reprioritizertr.c new file mode 100644 index 000000000000..913d8d5f5bfc --- /dev/null +++ b/nuttx/arch/sim/src/up_reprioritizertr.c @@ -0,0 +1,178 @@ +/**************************************************************************** + * up_reprioritizertr.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_reprioritize_rtr + * + * Description: + * Called when the priority of a running or + * ready-to-run task changes and the reprioritization will + * cause a context switch. Two cases: + * + * 1) The priority of the currently running task drops and the next + * task in the ready to run list has priority. + * 2) An idle, ready to run task's priority has been raised above the + * the priority of the current, running task and it now has the + * priority. + * + * Inputs: + * tcb: The TCB of the task that has been reprioritized + * priority: The new task priority + * + ****************************************************************************/ + +void up_reprioritize_rtr(_TCB *tcb, uint8_t priority) +{ + /* Verify that the caller is sane */ + + if (tcb->task_state < FIRST_READY_TO_RUN_STATE || + tcb->task_state > LAST_READY_TO_RUN_STATE +#if SCHED_PRIORITY_MIN > UINT8_MIN + || priority < SCHED_PRIORITY_MIN +#endif +#if SCHED_PRIORITY_MAX < UINT8_MAX + || priority > SCHED_PRIORITY_MAX +#endif + ) + { + PANIC(OSERR_BADREPRIORITIZESTATE); + } + else + { + _TCB *rtcb = (_TCB*)g_readytorun.head; + bool switch_needed; + + sdbg("TCB=%p PRI=%d\n", tcb, priority); + + /* Remove the tcb task from the ready-to-run list. + * sched_removereadytorun will return true if we just + * remove the head of the ready to run list. + */ + + switch_needed = sched_removereadytorun(tcb); + + /* Setup up the new task priority */ + + tcb->sched_priority = (uint8_t)priority; + + /* Return the task to the specified blocked task list. + * sched_addreadytorun will return true if the task was + * added to the new list. We will need to perform a context + * switch only if the EXCLUSIVE or of the two calls is non-zero + * (i.e., one and only one the calls changes the head of the + * ready-to-run list). + */ + + switch_needed ^= sched_addreadytorun(tcb); + + /* Now, perform the context switch if one is needed */ + + if (switch_needed) + { + /* If we are going to do a context switch, then now is the right + * time to add any pending tasks back into the ready-to-run list. + * task list now + */ + + if (g_pendingtasks.head) + { + sched_mergepending(); + } + + /* Copy the exception context into the TCB at the (old) head of the + * g_readytorun Task list. if up_setjmp returns a non-zero + * value, then this is really the previously running task restarting! + */ + + if (!up_setjmp(rtcb->xcp.regs)) + { + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + sdbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + sdbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } + } +} diff --git a/nuttx/arch/sim/src/up_romgetc.c b/nuttx/arch/sim/src/up_romgetc.c new file mode 100644 index 000000000000..9b75ecd6536d --- /dev/null +++ b/nuttx/arch/sim/src/up_romgetc.c @@ -0,0 +1,101 @@ +/**************************************************************************** + * arch/sim/src/up_romgetc.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include + +#ifdef CONFIG_ARCH_ROMGETC + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_romgetc + * + * Description: + * In Harvard architectures, data accesses and instruction accesses occur + * on different busses, perhaps concurrently. All data accesses are + * performed on the data bus unless special machine instructions are + * used to read data from the instruction address space. Also, in the + * typical MCU, the available SRAM data memory is much smaller that the + * non-volatile FLASH instruction memory. So if the application requires + * many constant strings, the only practical solution may be to store + * those constant strings in FLASH memory where they can only be accessed + * using architecture-specific machine instructions. + * + * A similar case is where strings are retained in "external" memory such + * as EEPROM or serial FLASH. This case is similar only in that again + * special operations are required to obtain the string data; it cannot + * be accessed directly from a string pointer. + * + * If CONFIG_ARCH_ROMGETC is defined, then the architecture logic must + * export the function up_romgetc(). up_romgetc() will simply read one + * byte of data from the instruction space. + * + * If CONFIG_ARCH_ROMGETC, certain C stdio functions are effected: (1) + * All format strings in printf, fprintf, sprintf, etc. are assumed to + * lie in FLASH (string arguments for %s are still assumed to reside in + * SRAM). And (2), the string argument to puts and fputs is assumed to + * reside in FLASH. Clearly, these assumptions may have to modified for + * the particular needs of your environment. There is no "one-size-fits-all" + * solution for this problem. + * + ****************************************************************************/ + +char up_romgetc(FAR const char *ptr) +{ + /* This is basically a No-Op if enabled in the simulation environment */ + + return *ptr; +} +#endif diff --git a/nuttx/arch/sim/src/up_schedulesigaction.c b/nuttx/arch/sim/src/up_schedulesigaction.c new file mode 100644 index 000000000000..871040a9490f --- /dev/null +++ b/nuttx/arch/sim/src/up_schedulesigaction.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * up_schedulesigaction.c + * + * Copyright (C) 2007-2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_schedule_sigaction + * + * Description: + * This function is called by the OS when one or more + * signal handling actions have been queued for execution. + * The architecture specific code must configure things so + * that the 'igdeliver' callback is executed on the thread + * specified by 'tcb' as soon as possible. + * + * This function may be called from interrupt handling logic. + * + * This operation should not cause the task to be unblocked + * nor should it cause any immediate execution of sigdeliver. + * Typically, a few cases need to be considered: + * + * (1) This function may be called from an interrupt handler + * During interrupt processing, all xcptcontext structures + * should be valid for all tasks. That structure should + * be modified to invoke sigdeliver() either on return + * from (this) interrupt or on some subsequent context + * switch to the recipient task. + * (2) If not in an interrupt handler and the tcb is NOT + * the currently executing task, then again just modify + * the saved xcptcontext structure for the recipient + * task so it will invoke sigdeliver when that task is + * later resumed. + * (3) If not in an interrupt handler and the tcb IS the + * currently executing task -- just call the signal + * handler now. + * + ****************************************************************************/ + +void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver) +{ + /* We don't have to anything complex for the simulated target */ + + if (tcb == (_TCB*)g_readytorun.head) + { + sigdeliver(tcb); + } + else + { + tcb->xcp.sigdeliver = sigdeliver; + } +} diff --git a/nuttx/arch/sim/src/up_setjmp.S b/nuttx/arch/sim/src/up_setjmp.S new file mode 100644 index 000000000000..a65317c65ab2 --- /dev/null +++ b/nuttx/arch/sim/src/up_setjmp.S @@ -0,0 +1,142 @@ +/************************************************************************** + * up_setjmp.S + * + * Copyright (C) 2007, 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + **************************************************************************/ + +/************************************************************************** + * Conditional Compilation Options + **************************************************************************/ + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include "up_internal.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +#ifdef __CYGWIN__ +# define SYMBOL(s) _##s +#else +# define SYMBOL(s) s +#endif + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + + .text + .globl SYMBOL(up_setjmp) +#ifndef __CYGWIN__ + .type SYMBOL(up_setjmp), @function +#endif +SYMBOL(up_setjmp): + + /* %ebx, %esi, %edi, and %ebp must be preserved. + * save %ebx, $esi, and %edi now... */ + + movl 4(%esp), %eax + movl %ebx, (JB_EBX)(%eax) + movl %esi, (JB_ESI)(%eax) + movl %edi, (JB_EDI)(%eax) + + /* Save the value of SP as will be after we return */ + + leal 4(%esp), %ecx + movl %ecx, (JB_SP)(%eax) + + /* Save the return PC */ + + movl 0(%esp), %ecx + movl %ecx, (JB_PC)(%eax) + + /* Save the framepointer */ + + movl %ebp, (JB_EBP)(%eax) + + /* And return 0 */ + + xorl %eax, %eax + ret +#ifndef __CYGWIN__ + .size SYMBOL(up_setjmp), . - SYMBOL(up_setjmp) +#endif + .globl SYMBOL(up_longjmp) +#ifndef __CYGWIN__ + .type SYMBOL(up_longjmp), @function +#endif +SYMBOL(up_longjmp): + movl 4(%esp), %ecx /* U_pthread_jmpbuf in %ecx. */ + movl 8(%esp), %eax /* Second argument is return value. */ + + /* Save the return address now. */ + + movl (JB_PC)(%ecx), %edx + + /* Restore registers. */ + + movl (JB_EBX)(%ecx), %ebx + movl (JB_ESI)(%ecx), %esi + movl (JB_EDI)(%ecx), %edi + movl (JB_EBP)(%ecx), %ebp + movl (JB_SP)(%ecx), %esp + + /* Jump to saved PC. */ + + jmp *%edx +#ifndef __CYGWIN__ + .size SYMBOL(up_longjmp), . - SYMBOL(up_longjmp) +#endif + diff --git a/nuttx/arch/sim/src/up_stdio.c b/nuttx/arch/sim/src/up_stdio.c new file mode 100644 index 000000000000..b6da9cb2fc3b --- /dev/null +++ b/nuttx/arch/sim/src/up_stdio.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * up_stdio.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +size_t up_hostread(void *buffer, size_t len) +{ + /* Just map to the host fread() */ + + return fread(buffer, 1, len, stdin); +} + +size_t up_hostwrite(const void *buffer, size_t len) +{ + /* Just map to the host fwrite() */ + + return fwrite(buffer, 1, len, stdout); +} + +int up_putc(int ch) +{ + /* Just map to the host fputc routine */ + + return fputc(ch, stdout); +} + diff --git a/nuttx/arch/sim/src/up_systemreset.c b/nuttx/arch/sim/src/up_systemreset.c new file mode 100644 index 000000000000..3ed160d11270 --- /dev/null +++ b/nuttx/arch/sim/src/up_systemreset.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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 + ****************************************************************************/ + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + + /**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public functions + ****************************************************************************/ + +void up_systemreset(void) +{ + exit(0); +} diff --git a/nuttx/arch/sim/src/up_tapdev.c b/nuttx/arch/sim/src/up_tapdev.c new file mode 100644 index 000000000000..f2bbcc14a224 --- /dev/null +++ b/nuttx/arch/sim/src/up_tapdev.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * up_tapdev.c + * + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Based on code from uIP which also has a BSD-like license: + * + * Copyright (c) 2001, Adam Dunkels. + * All rights reserved. + * + * 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. + * + ****************************************************************************/ + +#ifndef __CYGWIN__ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +extern int lib_rawprintf(const char *format, ...); +extern int uipdriver_setmacaddr(unsigned char *macaddr); + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +#define TAPDEV_DEBUG 1 + +#define DEVTAP "/dev/net/tun" + +#ifndef CONFIG_EXAMPLES_UIP_DHCPC +# define UIP_IPADDR0 192 +# define UIP_IPADDR1 168 +# define UIP_IPADDR2 0 +# define UIP_IPADDR3 128 +#else +# define UIP_IPADDR0 0 +# define UIP_IPADDR1 0 +# define UIP_IPADDR2 0 +# define UIP_IPADDR3 0 +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* Warning: This is very much Linux version specific! */ + +struct sel_arg_struct +{ + unsigned long n; + fd_set *inp; + fd_set *outp; + fd_set *exp; + struct timeval *tvp; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifdef TAPDEV_DEBUG +static int gdrop = 0; +#endif +static int gtapdevfd; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +#ifdef TAPDEV_DEBUG +static inline void dump_ethhdr(const char *msg, unsigned char *buf, int buflen) +{ + lib_rawprintf("TAPDEV: %s %d bytes\n", msg, buflen); + lib_rawprintf(" %02x:%02x:%02x:%02x:%02x:%02x %02x:%02x:%02x:%02x:%02x:%02x %02x%02x\n", + buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], + buf[6], buf[7], buf[8], buf[9], buf[10], buf[11], +#ifdef CONFIG_ENDIAN_BIG + buf[13], buf[12]); +#else + buf[12], buf[13]); +#endif +} +#else +# define dump_ethhdr(m,b,l) +#endif + +static int up_setmacaddr(void) +{ + int sockfd; + int ret = -1; + + /* Get a socket (only so that we get access to the INET subsystem) */ + + sockfd = socket(PF_INET, SOCK_DGRAM, 0); + if (sockfd >= 0) + { + struct ifreq req; + memset(&req, 0, sizeof(struct ifreq)); + + /* Put the driver name into the request */ + + strncpy(req.ifr_name, "tap0", IFNAMSIZ); + + /* Perform the ioctl to get the MAC address */ + + ret = ioctl(sockfd, SIOCGIFHWADDR, (unsigned long)&req); + if (!ret) + { + /* Set the MAC address */ + + ret = uipdriver_setmacaddr(&req.ifr_hwaddr.sa_data); + } + } + + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void tapdev_init(void) +{ + struct ifreq ifr; + char buf[1024]; + int ret; + + /* Open the tap device */ + + gtapdevfd = open(DEVTAP, O_RDWR, 0644); + if (gtapdevfd < 0) + { + lib_rawprintf("TAPDEV: open failed: %d\n", -gtapdevfd ); + return; + } + + /* Configure the tap device */ + + memset(&ifr, 0, sizeof(ifr)); + ifr.ifr_flags = IFF_TAP|IFF_NO_PI; + ret = ioctl(gtapdevfd, TUNSETIFF, (unsigned long) &ifr); + if (ret < 0) + { + lib_rawprintf("TAPDEV: ioctl failed: %d\n", -ret ); + return; + } + + /* Assign an IPv4 address to the tap device */ + + snprintf(buf, sizeof(buf), "/sbin/ifconfig tap0 inet %d.%d.%d.%d\n", + UIP_IPADDR0, UIP_IPADDR1, UIP_IPADDR2, UIP_IPADDR3); + system(buf); + + /* Set the MAC address */ + + up_setmacaddr(); +} + +unsigned int tapdev_read(unsigned char *buf, unsigned int buflen) +{ + fd_set fdset; + struct timeval tv; + int ret; + + /* We can't do anything if we failed to open the tap device */ + + if (gtapdevfd < 0) + { + return 0; + } + + /* Wait for data on the tap device (or a timeout) */ + + tv.tv_sec = 0; + tv.tv_usec = 1000; + + FD_ZERO(&fdset); + FD_SET(gtapdevfd, &fdset); + + ret = select(gtapdevfd + 1, &fdset, NULL, NULL, &tv); + if(ret == 0) + { + return 0; + } + + ret = read(gtapdevfd, buf, buflen); + if (ret < 0) + { + lib_rawprintf("TAPDEV: read failed: %d\n", -ret); + return 0; + } + + dump_ethhdr("read", buf, ret); + return ret; +} + +void tapdev_send(unsigned char *buf, unsigned int buflen) +{ + int ret; +#ifdef TAPDEV_DEBUG + lib_rawprintf("tapdev_send: sending %d bytes\n", buflen); + + gdrop++; + if(gdrop % 8 == 7) + { + lib_rawprintf("Dropped a packet!\n"); + return; + } +#endif + + ret = write(gtapdevfd, buf, buflen); + if (ret < 0) + { + lib_rawprintf("TAPDEV: write failed: %d", -ret); + exit(1); + } + dump_ethhdr("write", buf, buflen); +} + +#endif /* !__CYGWIN__ */ + + diff --git a/nuttx/arch/sim/src/up_touchscreen.c b/nuttx/arch/sim/src/up_touchscreen.c new file mode 100644 index 000000000000..cc40f50fecbb --- /dev/null +++ b/nuttx/arch/sim/src/up_touchscreen.c @@ -0,0 +1,811 @@ +/**************************************************************************** + * arch/sim/src/up_touchscreen.c + * + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifdef CONFIG_DISABLE_POLL +# undef CONFIG_SIM_TCNWAITERS +#else +# ifndef CONFIG_SIM_TCNWAITERS +# define CONFIG_SIM_TCNWAITERS 4 +# endif +#endif + +/* Driver support ***********************************************************/ +/* This format is used to construct the /dev/input[n] device driver path. It + * defined here so that it will be used consistently in all places. + */ + +#define DEV_FORMAT "/dev/input%d" +#define DEV_NAMELEN 16 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* This describes the state of one contact */ + +enum up_contact_3 +{ + CONTACT_NONE = 0, /* No contact */ + CONTACT_DOWN, /* First contact */ + CONTACT_MOVE, /* Same contact, possibly different position */ + CONTACT_UP, /* Contact lost */ +}; + +/* This structure describes the results of one touchscreen sample */ + +struct up_sample_s +{ + uint8_t id; /* Sampled touch point ID */ + uint8_t contact; /* Contact state (see enum up_contact_e) */ + uint16_t x; /* Measured X position */ + uint16_t y; /* Measured Y position */ +}; + +/* This structure describes the state of one touchscreen driver instance */ + +struct up_dev_s +{ + volatile uint8_t nwaiters; /* Number of threads waiting for touchscreen data */ + uint8_t id; /* Current touch point ID */ + uint8_t minor; /* Minor device number */ + volatile bool penchange; /* An unreported event is buffered */ + sem_t devsem; /* Manages exclusive access to this structure */ + sem_t waitsem; /* Used to wait for the availability of data */ + + struct up_sample_s sample; /* Last sampled touch point data */ + + /* The following is a list if poll structures of threads waiting for + * driver events. The 'struct pollfd' reference for each open is also + * retained in the f_priv field of the 'struct file'. + */ + +#ifndef CONFIG_DISABLE_POLL + struct pollfd *fds[CONFIG_SIM_TCNWAITERS]; +#endif +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static void up_notify(FAR struct up_dev_s *priv); +static int up_sample(FAR struct up_dev_s *priv, + FAR struct up_sample_s *sample); +static int up_waitsample(FAR struct up_dev_s *priv, + FAR struct up_sample_s *sample); + +/* Character driver methods */ + +static int up_open(FAR struct file *filep); +static int up_close(FAR struct file *filep); +static ssize_t up_read(FAR struct file *filep, FAR char *buffer, size_t len); +static int up_ioctl(FAR struct file *filep, int cmd, unsigned long arg); +#ifndef CONFIG_DISABLE_POLL +static int up_poll(FAR struct file *filep, struct pollfd *fds, bool setup); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* This the the vtable that supports the character driver interface */ + +static const struct file_operations up_fops = +{ + up_open, /* open */ + up_close, /* close */ + up_read, /* read */ + 0, /* write */ + 0, /* seek */ + up_ioctl /* ioctl */ +#ifndef CONFIG_DISABLE_POLL + , up_poll /* poll */ +#endif +}; + +/* Only one simulated touchscreen is supported o the the driver state + * structure may as well be pre-allocated. + */ + +static struct up_dev_s g_simtouchscreen; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_notify + ****************************************************************************/ + +static void up_notify(FAR struct up_dev_s *priv) +{ +#ifndef CONFIG_DISABLE_POLL + int i; +#endif + + /* If there are threads waiting for read data, then signal one of them + * that the read data is available. + */ + + ivdbg("contact=%d nwaiters=%d\n", priv->sample.contact, priv->nwaiters); + if (priv->nwaiters > 0) + { + /* After posting this semaphore, we need to exit because the touchscreen + * is no longer avaialable. + */ + + sem_post(&priv->waitsem); + } + + /* If there are threads waiting on poll() for touchscreen data to become availabe, + * then wake them up now. NOTE: we wake up all waiting threads because we + * do not know that they are going to do. If they all try to read the data, + * then some make end up blocking after all. + */ + +#ifndef CONFIG_DISABLE_POLL + for (i = 0; i < CONFIG_SIM_TCNWAITERS; i++) + { + struct pollfd *fds = priv->fds[i]; + if (fds) + { + fds->revents |= POLLIN; + ivdbg("Report events: %02x\n", fds->revents); + sem_post(fds->sem); + } + } +#endif +} + +/**************************************************************************** + * Name: up_sample + ****************************************************************************/ + +static int up_sample(FAR struct up_dev_s *priv, + FAR struct up_sample_s *sample) +{ + int ret = -EAGAIN; + + /* Is there new touchscreen sample data available? */ + + ivdbg("penchange=%d contact=%d id=%d\n", + priv->penchange, sample->contact, priv->id); + + if (priv->penchange) + { + /* Yes.. the state has changed in some way. Return a copy of the + * sampled data. + */ + + memcpy(sample, &priv->sample, sizeof(struct up_sample_s )); + + /* Now manage state transitions */ + + if (sample->contact == CONTACT_UP) + { + /* Next.. no contract. Increment the ID so that next contact ID will be unique */ + + priv->sample.contact = CONTACT_NONE; + priv->id++; + } + else if (sample->contact == CONTACT_DOWN) + { + /* First report -- next report will be a movement */ + + priv->sample.contact = CONTACT_MOVE; + } + + priv->penchange = false; + ivdbg("penchange=%d contact=%d id=%d\n", + priv->penchange, priv->sample.contact, priv->id); + + ret = OK; + } + + return ret; +} + +/**************************************************************************** + * Name: up_waitsample + ****************************************************************************/ + +static int up_waitsample(FAR struct up_dev_s *priv, + FAR struct up_sample_s *sample) +{ + irqstate_t flags; + int ret; + + /* Interrupts me be disabled when this is called to (1) prevent posting + * of semphores from interrupt handlers, and (2) to prevent sampled data + * from changing until it has been reported. + * + * In addition, we will also disable pre-emption to prevent other threads + * from getting control while we muck with the semaphores. + */ + + sched_lock(); + flags = irqsave(); + + /* Now release the semaphore that manages mutually exclusive access to + * the device structure. This may cause other tasks to become ready to + * run, but they cannot run yet because pre-emption is disabled. + */ + + sem_post(&priv->devsem); + + /* Try to get the a sample... if we cannot, then wait on the semaphore + * that is posted when new sample data is available. + */ + + while (up_sample(priv, sample) < 0) + { + /* Wait for a change in the touchscreen state */ + + ivdbg("Waiting...\n"); + priv->nwaiters++; + ret = sem_wait(&priv->waitsem); + priv->nwaiters--; + ivdbg("Awakened...\n"); + + if (ret < 0) + { + /* If we are awakened by a signal, then we need to return + * the failure now. + */ + + DEBUGASSERT(errno == EINTR); + ret = -EINTR; + goto errout; + } + } + + /* Re-acquire the the semaphore that manages mutually exclusive access to + * the device structure. We may have to wait here. But we have our sample. + * Interrupts and pre-emption will be re-enabled while we wait. + */ + + ret = sem_wait(&priv->devsem); + +errout: + /* Then re-enable interrupts. We might get interrupt here and there + * could be a new sample. But no new threads will run because we still + * have pre-emption disabled. + */ + + irqrestore(flags); + + /* Restore pre-emption. We might get suspended here but that is okay + * because we already have our sample. Note: this means that if there + * were two threads reading from the touchscreen for some reason, the data + * might be read out of order. + */ + + sched_unlock(); + return ret; +} + +/**************************************************************************** + * Name: up_open + ****************************************************************************/ + +static int up_open(FAR struct file *filep) +{ + ivdbg("Opening...\n"); + return OK; +} + +/**************************************************************************** + * Name: up_close + ****************************************************************************/ + +static int up_close(FAR struct file *filep) +{ + ivdbg("Closing...\n"); + return OK; +} + +/**************************************************************************** + * Name: up_read + ****************************************************************************/ + +static ssize_t up_read(FAR struct file *filep, FAR char *buffer, size_t len) +{ + FAR struct inode *inode; + FAR struct up_dev_s *priv; + FAR struct touch_sample_s *report; + struct up_sample_s sample; + int ret; + + ivdbg("len=%d\n", len); + + DEBUGASSERT(filep); + inode = filep->f_inode; + + DEBUGASSERT(inode && inode->i_private); + priv = (FAR struct up_dev_s *)inode->i_private; + + /* Verify that the caller has provided a buffer large enough to receive + * the touch data. + */ + + if (len < SIZEOF_TOUCH_SAMPLE_S(1)) + { + /* We could provide logic to break up a touch report into segments and + * handle smaller reads... but why? + */ + + return -ENOSYS; + } + + /* Get exclusive access to the driver data structure */ + + ret = sem_wait(&priv->devsem); + if (ret < 0) + { + /* This should only happen if the wait was canceled by an signal */ + + DEBUGASSERT(errno == EINTR); + return -EINTR; + } + + /* Try to read sample data. */ + + ret = up_sample(priv, &sample); + if (ret < 0) + { + /* Sample data is not available now. We would ave to wait to get + * receive sample data. If the user has specified the O_NONBLOCK + * option, then just return an error. + */ + + if (filep->f_oflags & O_NONBLOCK) + { + ret = -EAGAIN; + goto errout; + } + + /* Wait for sample data */ + + ret = up_waitsample(priv, &sample); + if (ret < 0) + { + /* We might have been awakened by a signal */ + + goto errout; + } + } + + /* In any event, we now have sampled touchscreen data that we can report + * to the caller. + */ + + report = (FAR struct touch_sample_s *)buffer; + memset(report, 0, SIZEOF_TOUCH_SAMPLE_S(1)); + report->npoints = 1; + report->point[0].id = priv->id; + report->point[0].x = sample.x; + report->point[0].y = sample.y; + report->point[0].h = 1; + report->point[0].w = 1; + report->point[0].pressure = 42; + + /* Report the appropriate flags */ + + if (sample.contact == CONTACT_UP) + { + /* Pen is now up */ + + report->point[0].flags = TOUCH_UP | TOUCH_ID_VALID; + } + else if (sample.contact == CONTACT_DOWN) + { + /* First contact */ + + report->point[0].flags = TOUCH_DOWN | TOUCH_ID_VALID | TOUCH_POS_VALID | TOUCH_PRESSURE_VALID; + } + else /* if (sample->contact == CONTACT_MOVE) */ + { + /* Movement of the same contact */ + + report->point[0].flags = TOUCH_MOVE | TOUCH_ID_VALID | TOUCH_POS_VALID | TOUCH_PRESSURE_VALID; + } + + ret = SIZEOF_TOUCH_SAMPLE_S(1); + +errout: + ivdbg("Returning %d\n", ret); + sem_post(&priv->devsem); + return ret; +} + +/**************************************************************************** + * Name:up_ioctl + ****************************************************************************/ + +static int up_ioctl(FAR struct file *filep, int cmd, unsigned long arg) +{ + FAR struct inode *inode; + FAR struct up_dev_s *priv; + int ret; + + ivdbg("cmd: %d arg: %ld\n", cmd, arg); + DEBUGASSERT(filep); + inode = filep->f_inode; + + DEBUGASSERT(inode && inode->i_private); + priv = (FAR struct up_dev_s *)inode->i_private; + + /* Get exclusive access to the driver data structure */ + + ret = sem_wait(&priv->devsem); + if (ret < 0) + { + /* This should only happen if the wait was canceled by an signal */ + + DEBUGASSERT(errno == EINTR); + return -EINTR; + } + + /* Process the IOCTL by command */ + + switch (cmd) + { + default: + ret = -ENOTTY; + break; + } + + sem_post(&priv->devsem); + return ret; +} + +/**************************************************************************** + * Name: up_poll + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_POLL +static int up_poll(FAR struct file *filep, FAR struct pollfd *fds, + bool setup) +{ + FAR struct inode *inode; + FAR struct up_dev_s *priv; + int ret = OK; + int i; + + ivdbg("setup: %d\n", (int)setup); + DEBUGASSERT(filep && fds); + inode = filep->f_inode; + + DEBUGASSERT(inode && inode->i_private); + priv = (FAR struct up_dev_s *)inode->i_private; + + /* Are we setting up the poll? Or tearing it down? */ + + ret = sem_wait(&priv->devsem); + if (ret < 0) + { + /* This should only happen if the wait was canceled by an signal */ + + DEBUGASSERT(errno == EINTR); + return -EINTR; + } + + if (setup) + { + /* Ignore waits that do not include POLLIN */ + + if ((fds->revents & POLLIN) == 0) + { + ret = -EDEADLK; + goto errout; + } + + /* This is a request to set up the poll. Find an available + * slot for the poll structure reference + */ + + for (i = 0; i < CONFIG_SIM_TCNWAITERS; i++) + { + /* Find an available slot */ + + if (!priv->fds[i]) + { + /* Bind the poll structure and this slot */ + + priv->fds[i] = fds; + fds->priv = &priv->fds[i]; + break; + } + } + + if (i >= CONFIG_SIM_TCNWAITERS) + { + fds->priv = NULL; + ret = -EBUSY; + goto errout; + } + + /* Should we immediately notify on any of the requested events? */ + + if (priv->penchange) + { + up_notify(priv); + } + } + else if (fds->priv) + { + /* This is a request to tear down the poll. */ + + struct pollfd **slot = (struct pollfd **)fds->priv; + DEBUGASSERT(slot != NULL); + + /* Remove all memory of the poll setup */ + + *slot = NULL; + fds->priv = NULL; + } + +errout: + sem_post(&priv->devsem); + return ret; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: arch_tcinitialize + * + * Description: + * Configure the simulated touchscreen. This will register the driver as + * /dev/inputN where N is the minor device number + * + * Input Parameters: + * minor - The input device minor number + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int arch_tcinitialize(int minor) +{ + FAR struct up_dev_s *priv = ( FAR struct up_dev_s *)&g_simtouchscreen; + char devname[DEV_NAMELEN]; + int ret; + + ivdbg("minor: %d\n", minor); + + /* Debug-only sanity checks */ + + DEBUGASSERT(minor >= 0 && minor < 100); + + /* Initialize the touchscreen device driver instance */ + + memset(priv, 0, sizeof(struct up_dev_s)); + sem_init(&priv->devsem, 0, 1); /* Initialize device structure semaphore */ + sem_init(&priv->waitsem, 0, 0); /* Initialize pen event wait semaphore */ + + priv->minor = minor; + + /* Register the device as an input device */ + + (void)snprintf(devname, DEV_NAMELEN, DEV_FORMAT, minor); + ivdbg("Registering %s\n", devname); + + ret = register_driver(devname, &up_fops, 0666, priv); + if (ret < 0) + { + idbg("register_driver() failed: %d\n", ret); + goto errout_with_priv; + } + + /* Enable X11 event processing from the IDLE loop */ + + g_eventloop = 1; + + /* And return success */ + + return OK; + +errout_with_priv: + sem_destroy(&priv->waitsem); + sem_destroy(&priv->devsem); + return ret; +} + +/**************************************************************************** + * Name: arch_tcuninitialize + * + * Description: + * Uninitialized the simulated touchscreen + * + * Input Parameters: + * None + * + * Returned Value: + * None. + * + ****************************************************************************/ + +void arch_tcuninitialize(void) +{ + FAR struct up_dev_s *priv = ( FAR struct up_dev_s *)&g_simtouchscreen; + char devname[DEV_NAMELEN]; + int ret; + + /* Get exclusive access */ + + do + { + ret = sem_wait(&priv->devsem); + if (ret < 0) + { + /* This should only happen if the wait was canceled by an signal */ + + DEBUGASSERT(errno == EINTR); + } + } + while (ret != OK); + + /* Stop the event loop (Hmm.. the caller must be sure that there are no + * open references to the touchscreen driver. This might better be + * done in close() using a reference count). + */ + + g_eventloop = 0; + + /* Un-register the device*/ + + (void)snprintf(devname, DEV_NAMELEN, DEV_FORMAT, priv->minor); + ivdbg("Un-registering %s\n", devname); + + ret = unregister_driver(devname); + if (ret < 0) + { + idbg("uregister_driver() failed: %d\n", ret); + } + + /* Clean up any resources. Ouch! While we are holding the semaphore? */ + + sem_destroy(&priv->waitsem); + sem_destroy(&priv->devsem); +} + +/**************************************************************************** + * Name: up_buttonevent + ****************************************************************************/ + +int up_buttonevent(int x, int y, int buttons) +{ + FAR struct up_dev_s *priv = (FAR struct up_dev_s *)&g_simtouchscreen; + bool pendown; /* true: pen is down */ + + ivdbg("x=%d y=%d buttons=%02x\n", x, y, buttons); + ivdbg("contact=%d nwaiters=%d\n", priv->sample.contact, priv->nwaiters); + + /* Any button press will count as pendown. */ + + pendown = (buttons != 0); + + /* Handle the change from pen down to pen up */ + + if (!pendown) + { + /* Ignore the pend up if the pen was already up (CONTACT_NONE == pen up and + * already reported. CONTACT_UP == pen up, but not reported) + */ + + if (priv->sample.contact == CONTACT_NONE) + { + return OK; + } + + /* Not yet reported */ + + priv->sample.contact = CONTACT_UP; + } + else + { + /* Save the measurements */ + + priv->sample.x = x; + priv->sample.y = y; + + /* Note the availability of new measurements */ + /* If this is the first (acknowledged) pen down report, then report + * this as the first contact. If contact == CONTACT_DOWN, it will be + * set to set to CONTACT_MOVE after the contact is first sampled. + */ + + if (priv->sample.contact != CONTACT_MOVE) + { + /* First contact */ + + priv->sample.contact = CONTACT_DOWN; + } + } + + /* Indicate the availability of new sample data for this ID */ + + priv->sample.id = priv->id; + priv->penchange = true; + + /* Notify any waiters that new touchscreen data is available */ + + up_notify(priv); + return OK; +} + diff --git a/nuttx/arch/sim/src/up_uipdriver.c b/nuttx/arch/sim/src/up_uipdriver.c new file mode 100644 index 000000000000..99f37b22ffa4 --- /dev/null +++ b/nuttx/arch/sim/src/up_uipdriver.c @@ -0,0 +1,230 @@ +/**************************************************************************** + * up_uipdriver.c + * + * Copyright (C) 2007, 2009-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Based on code from uIP which also has a BSD-like license: + * + * Copyright (c) 2001, Adam Dunkels. + * All rights reserved. + * + * 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 + ****************************************************************************/ + +#include + +#ifdef CONFIG_NET + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +#define BUF ((struct ether_header*)g_sim_dev.d_buf) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct timer +{ + uint32_t interval; + uint32_t start; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct timer g_periodic_timer; +static struct uip_driver_s g_sim_dev; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void timer_set(struct timer *t, unsigned int interval) +{ + t->interval = interval; + t->start = up_getwalltime(); +} + +static bool timer_expired( struct timer *t ) +{ + return (up_getwalltime() - t->start) >= t->interval; +} + +void timer_reset(struct timer *t) +{ + t->start += t->interval; +} + +#ifdef CONFIG_NET_PROMISCUOUS +# define up_comparemac(a,b) (0) +#else +static inline int up_comparemac(uint8_t *paddr1, struct ether_addr *paddr2) +{ + return memcmp(paddr1, paddr2->ether_addr_octet, ETHER_ADDR_LEN); +} +#endif + +static int sim_uiptxpoll(struct uip_driver_s *dev) +{ + /* If the polling resulted in data that should be sent out on the network, + * the field d_len is set to a value > 0. + */ + + if (g_sim_dev.d_len > 0) + { + uip_arp_out(&g_sim_dev); + netdev_send(g_sim_dev.d_buf, g_sim_dev.d_len); + } + + /* If zero is returned, the polling will continue until all connections have + * been examined. + */ + + return 0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void uipdriver_loop(void) +{ + /* netdev_read will return 0 on a timeout event and >0 on a data received event */ + + g_sim_dev.d_len = netdev_read((unsigned char*)g_sim_dev.d_buf, CONFIG_NET_BUFSIZE); + + /* Disable preemption through to the following so that it behaves a little more + * like an interrupt (otherwise, the following logic gets pre-empted an behaves + * oddly. + */ + + sched_lock(); + if (g_sim_dev.d_len > 0) + { + /* Data received event. Check for valid Ethernet header with destination == our + * MAC address + */ + + if (g_sim_dev.d_len > UIP_LLH_LEN && up_comparemac(BUF->ether_dhost, &g_sim_dev.d_mac) == 0) + { + /* We only accept IP packets of the configured type and ARP packets */ + +#ifdef CONFIG_NET_IPv6 + if (BUF->ether_type == htons(UIP_ETHTYPE_IP6)) +#else + if (BUF->ether_type == htons(UIP_ETHTYPE_IP)) +#endif + { + uip_arp_ipin(&g_sim_dev); + uip_input(&g_sim_dev); + + /* If the above function invocation resulted in data that + * should be sent out on the network, the global variable + * d_len is set to a value > 0. + */ + + if (g_sim_dev.d_len > 0) + { + uip_arp_out(&g_sim_dev); + netdev_send(g_sim_dev.d_buf, g_sim_dev.d_len); + } + } + else if (BUF->ether_type == htons(UIP_ETHTYPE_ARP)) + { + uip_arp_arpin(&g_sim_dev); + + /* If the above function invocation resulted in data that + * should be sent out on the network, the global variable + * d_len is set to a value > 0. + */ + + if (g_sim_dev.d_len > 0) + { + netdev_send(g_sim_dev.d_buf, g_sim_dev.d_len); + } + } + } + } + + /* Otherwise, it must be a timeout event */ + + else if (timer_expired(&g_periodic_timer)) + { + timer_reset(&g_periodic_timer); + uip_timer(&g_sim_dev, sim_uiptxpoll, 1); + } + sched_unlock(); +} + +int uipdriver_init(void) +{ + /* Internal initalization */ + + timer_set(&g_periodic_timer, 500); + netdev_init(); + + /* Register the device with the OS so that socket IOCTLs can be performed */ + + (void)netdev_register(&g_sim_dev); + return OK; +} + +int uipdriver_setmacaddr(unsigned char *macaddr) +{ + (void)memcpy(g_sim_dev.d_mac.ether_addr_octet, macaddr, IFHWADDRLEN); + return 0; +} + +#endif /* CONFIG_NET */ + diff --git a/nuttx/arch/sim/src/up_unblocktask.c b/nuttx/arch/sim/src/up_unblocktask.c new file mode 100644 index 000000000000..04b6455f31c3 --- /dev/null +++ b/nuttx/arch/sim/src/up_unblocktask.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * up_unblocktask.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "clock_internal.h" +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_unblock_task + * + * Description: + * A task is currently in an inactive task list + * but has been prepped to execute. Move the TCB to the + * ready-to-run list, restore its context, and start execution. + * + * Inputs: + * tcb: Refers to the tcb to be unblocked. This tcb is + * in one of the waiting tasks lists. It must be moved to + * the ready-to-run list and, if it is the highest priority + * ready to run taks, executed. + * + ****************************************************************************/ + +void up_unblock_task(_TCB *tcb) +{ + /* Verify that the context switch can be performed */ + if ((tcb->task_state < FIRST_BLOCKED_STATE) || + (tcb->task_state > LAST_BLOCKED_STATE)) + { + PANIC(OSERR_BADUNBLOCKSTATE); + } + else + { + _TCB *rtcb = (_TCB*)g_readytorun.head; + + sdbg("Unblocking TCB=%p\n", tcb); + + /* Remove the task from the blocked task list */ + + sched_removeblocked(tcb); + + /* Reset its timeslice. This is only meaningful for round + * robin tasks but it doesn't here to do it for everything + */ + +#if CONFIG_RR_INTERVAL > 0 + tcb->timeslice = CONFIG_RR_INTERVAL / MSEC_PER_TICK; +#endif + + /* Add the task in the correct location in the prioritized + * g_readytorun task list + */ + + if (sched_addreadytorun(tcb)) + { + /* The currently active task has changed! Copy the exception context + * into the TCB of the task that was previously active. if + * up_setjmp returns a non-zero value, then this is really the + * previously running task restarting! + */ + + if (!up_setjmp(rtcb->xcp.regs)) + { + /* Restore the exception context of the new task that is ready to + * run (probably tcb). This is the new rtcb at the head of the + * g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + sdbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + sdbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } + } +} diff --git a/nuttx/arch/sim/src/up_usestack.c b/nuttx/arch/sim/src/up_usestack.c new file mode 100644 index 000000000000..aa832ff90258 --- /dev/null +++ b/nuttx/arch/sim/src/up_usestack.c @@ -0,0 +1,101 @@ +/**************************************************************************** + * arch/sim/src/up_usestack.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include +#include +#include +#include +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_use_stack + * + * Description: + * Setup up stack-related information in the TCB + * using pre-allocated stack memory + * + * The following TCB fields must be initialized: + * adj_stack_size: Stack size after adjustment for hardware, + * processor, etc. This value is retained only for debug + * purposes. + * stack_alloc_ptr: Pointer to allocated stack + * adj_stack_ptr: Adjusted stack_alloc_ptr for HW. The + * initial value of the stack pointer. + * + * Inputs: + * tcb: The TCB of new task + * stack_size: The allocated stack size. + * + ****************************************************************************/ + +int up_use_stack(_TCB *tcb, void *stack, size_t stack_size) +{ + /* Move up to next even word boundary if necessary */ + + size_t adj_stack_size = stack_size & ~3; + size_t adj_stack_words = adj_stack_size >> 2; + + /* This is the address of the last word in the allocation */ + + size_t *adj_stack_ptr = &((size_t*)stack)[adj_stack_words - 1]; + + /* Save the values in the TCB */ + + tcb->adj_stack_size = adj_stack_size; + tcb->stack_alloc_ptr = stack; + tcb->adj_stack_ptr = adj_stack_ptr; + return OK; +} diff --git a/nuttx/arch/sim/src/up_wpcap.c b/nuttx/arch/sim/src/up_wpcap.c new file mode 100644 index 000000000000..62f0b6ad0380 --- /dev/null +++ b/nuttx/arch/sim/src/up_wpcap.c @@ -0,0 +1,312 @@ +/**************************************************************************** + * up_wcap.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Based on code from uIP which also has a BSD-like license: + * + * Copyright (c) 2007, Swedish Institute of Computer Science. + * All rights reserved. + * + * 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 of the Institute 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 INSTITUTE 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 INSTITUTE 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. + * + ****************************************************************************/ + +#ifdef __CYGWIN__ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#define WIN32_LEAN_AND_MEAN +#define _WIN32_WINNT 0x0501 +#include +#include +#include + +#include +#include +#include + +extern int uipdriver_setmacaddr(unsigned char *macaddr); + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define BUF ((struct uip_eth_hdr *)&uip_buf[0]) + +#ifndef CONFIG_EXAMPLES_UIP_DHCPC +# define UIP_IPADDR (10 << 24 | 0 << 16 | 0 << 8 | 1) +#else +# define UIP_IPADDR (0) +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +__attribute__ ((dllimport)) extern char **__argv[]; + +struct pcap; + +struct pcap_if +{ + struct pcap_if *next; + char *name; + char *description; + struct pcap_addr + { + struct pcap_addr *next; + struct sockaddr *addr; + struct sockaddr *netmask; + struct sockaddr *broadaddr; + struct sockaddr *dstaddr; + } *addresses; + DWORD flags; +}; + +struct pcap_pkthdr +{ + struct timeval ts; + DWORD caplen; + DWORD len; +}; + +/* DLL function types (for casting) */ + +typedef int (*pcap_findalldevs_t)(struct pcap_if **, char *); +typedef struct pcap *(*pcap_open_live_t)(char *, int, int, int, char *); +typedef int (*pcap_next_ex_t)(struct pcap *, struct pcap_pkthdr **, + unsigned char **); +typedef int (*pcap_sendpacket_t)(struct pcap *, unsigned char *, int); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +HMODULE wpcap; +static struct pcap *pcap; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static pcap_findalldevs_t pcap_findalldevs; +static pcap_open_live_t pcap_open_live; +static pcap_next_ex_t pcap_next_ex; +static pcap_sendpacket_t pcap_sendpacket; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void error_exit(char *message) +{ + printf("error_exit: %s", message); + exit(EXIT_FAILURE); +} + +static void init_pcap(struct in_addr addr) +{ + struct pcap_if *interfaces; + char error[256]; + + if (pcap_findalldevs(&interfaces, error) == -1) + { + error_exit(error); + } + + while (interfaces != NULL) + { + printf("init_pcap: found interface: %s\n", interfaces->description); + + if (interfaces->addresses != NULL && + interfaces->addresses->addr != NULL && + interfaces->addresses->addr->sa_family == AF_INET) + { + + struct in_addr interface_addr; + interface_addr = + ((struct sockaddr_in *)interfaces->addresses->addr)->sin_addr; + printf("init_pcap: with address: %s\n", inet_ntoa(interface_addr)); + + if (interface_addr.s_addr == addr.s_addr) + { + break; + } + } + interfaces = interfaces->next; + } + + if (interfaces == NULL) + { + error_exit("No interface found with IP address\n"); + } + + pcap = pcap_open_live(interfaces->name, NETDEV_BUFSIZE, 0, -1, error); + if (pcap == NULL) + { + error_exit(error); + } +} + +static void set_ethaddr(struct in_addr addr) +{ + PIP_ADAPTER_ADDRESSES adapters; + ULONG size = 0; + + if (GetAdaptersAddresses(AF_INET, GAA_FLAG_SKIP_ANYCAST | + GAA_FLAG_SKIP_MULTICAST | + GAA_FLAG_SKIP_DNS_SERVER, + NULL, NULL, &size) != ERROR_BUFFER_OVERFLOW) + { + error_exit("error on access to adapter list size\n"); + } + adapters = alloca(size); + if (GetAdaptersAddresses(AF_INET, GAA_FLAG_SKIP_ANYCAST | + GAA_FLAG_SKIP_MULTICAST | + GAA_FLAG_SKIP_DNS_SERVER, + NULL, adapters, &size) != ERROR_SUCCESS) + { + error_exit("error on access to adapter list\n"); + } + + while (adapters != NULL) + { + + char buffer[256]; + WideCharToMultiByte(CP_ACP, 0, adapters->Description, -1, + buffer, sizeof(buffer), NULL, NULL); + printf("set_ethaddr: found adapter: %s\n", buffer); + + if (adapters->FirstUnicastAddress != NULL && + adapters->FirstUnicastAddress->Address.lpSockaddr != NULL && + adapters->FirstUnicastAddress->Address.lpSockaddr->sa_family == + AF_INET) + { + + struct in_addr adapter_addr; + adapter_addr = + ((struct sockaddr_in *)adapters->FirstUnicastAddress->Address. + lpSockaddr)->sin_addr; + printf("set_ethaddr: with address: %s\n", inet_ntoa(adapter_addr)); + + if (adapter_addr.s_addr == addr.s_addr) + { + if (adapters->PhysicalAddressLength != 6) + { + error_exit + ("ip addr specified does not belong to an ethernet card\n"); + } + printf + ("set_ethaddr: ethernetaddr: %02X-%02X-%02X-%02X-%02X-%02X\n", + adapters->PhysicalAddress[0], adapters->PhysicalAddress[1], + adapters->PhysicalAddress[2], adapters->PhysicalAddress[3], + adapters->PhysicalAddress[4], adapters->PhysicalAddress[5]); + + (void)uipdriver_setmacaddr(adapters->PhysicalAddress); + break; + } + } + adapters = adapters->Next; + } + + if (adapters == NULL) + { + error_exit("No adaptor found with IP address\n"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void wpcap_init(void) +{ + struct in_addr addr; + FARPROC dlladdr; + + addr.s_addr = htonl(UIP_IPADDR); + printf("wpcap_init: IP address: %s\n", inet_ntoa(addr)); + + wpcap = LoadLibrary("wpcap.dll"); + dlladdr = GetProcAddress(wpcap, "pcap_findalldevs"); + pcap_findalldevs = (pcap_findalldevs_t)dlladdr; + + dlladdr = GetProcAddress(wpcap, "pcap_open_live"); + pcap_open_live = (pcap_open_live_t)dlladdr; + + dlladdr = GetProcAddress(wpcap, "pcap_next_ex"); + pcap_next_ex = (pcap_next_ex_t)dlladdr; + + dlladdr = GetProcAddress(wpcap, "pcap_sendpacket"); + pcap_sendpacket = (pcap_sendpacket_t)dlladdr; + + if (pcap_findalldevs == NULL || pcap_open_live == NULL || + pcap_next_ex == NULL || pcap_sendpacket == NULL) + { + error_exit("error on access to winpcap library\n"); + } + + init_pcap(addr); + set_ethaddr(addr); +} + +unsigned int wpcap_read(unsigned char *buf, unsigned int buflen) +{ + struct pcap_pkthdr *packet_header; + unsigned char *packet; + + switch (pcap_next_ex(pcap, &packet_header, &packet)) + { + case -1: + error_exit("error on read\n"); + case 0: + return 0; + } + + if (packet_header->caplen > buflen) + { + return 0; + } + + memcpy(buf, packet, packet_header->caplen); + return packet_header->caplen; +} + +void wpcap_send(unsigned char *buf, unsigned int buflen) +{ + if (pcap_sendpacket(pcap, buf, buflen) == -1) + { + error_exit("error on send\n"); + } +} + +#endif /* __CYGWIN__ */ diff --git a/nuttx/arch/sim/src/up_x11eventloop.c b/nuttx/arch/sim/src/up_x11eventloop.c new file mode 100644 index 000000000000..e2354dbecaad --- /dev/null +++ b/nuttx/arch/sim/src/up_x11eventloop.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * arch/sim/src/up_x11eventloop.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ***************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +extern int up_buttonevent(int x, int y, int buttons); + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* Defined in up_x11framebuffer.c */ + +extern Display *g_display; + +volatile int g_eventloop; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ***************************************************************************/ + +/**************************************************************************** + * Name: up_buttonmap + ***************************************************************************/ + +static int up_buttonmap(int state) +{ + /* Remove any X11 dependencies. Just maps Button1Mask to bit 0. */ + + return ((state & Button1Mask) != 0) ? 1 : 0; +} + +/**************************************************************************** + * Public Functions + ***************************************************************************/ + +/**************************************************************************** + * Name: up_x11events + * + * Description: + * Called periodically from the IDLE loop to check for queued X11 events. + * + ***************************************************************************/ + +void up_x11events(void) +{ + XEvent event; + + /* Check if there are any pending, queue X11 events. */ + + if (XPending(g_display) > 0) + { + /* Yes, get the event (this should not block since we know there are + * pending events) + */ + + XNextEvent(g_display, &event); + + /* Then process the event */ + + switch (event.type) + { + case MotionNotify : /* Enabled by ButtonMotionMask */ + { + up_buttonevent(event.xmotion.x, event.xmotion.y, + up_buttonmap(event.xmotion.state)); + } + break; + + case ButtonPress : /* Enabled by ButtonPressMask */ + case ButtonRelease : /* Enabled by ButtonReleaseMask */ + { + up_buttonevent(event.xbutton.x, event.xbutton.y, + up_buttonmap(event.xbutton.state)); + } + break; + + default : + break; + } + } +} diff --git a/nuttx/arch/sim/src/up_x11framebuffer.c b/nuttx/arch/sim/src/up_x11framebuffer.c new file mode 100644 index 000000000000..a035e2f3a2e9 --- /dev/null +++ b/nuttx/arch/sim/src/up_x11framebuffer.c @@ -0,0 +1,477 @@ +/**************************************************************************** + * arch/sim/src/up_x11framebuffer.c + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#define CONFIG_SIM_X11NOSHM 1 + +#include +#include +#include +#include +#include +#include + +#ifndef CONFIG_SIM_X11NOSHM +# include +# include +#endif + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Declarations + ***************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* Also used in up_x11eventloop */ + +Display *g_display; +int g_x11initialized; + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +static int g_screen; +static Window g_window; +static GC g_gc; +#ifndef CONFIG_SIM_X11NOSHM +static XShmSegmentInfo g_xshminfo; +static int g_xerror; +#endif +static XImage *g_image; +static unsigned char *g_framebuffer; +static unsigned short g_fbpixelwidth; +static unsigned short g_fbpixelheight; +static int g_shmcheckpoint = 0; +static int b_useshm; + +/**************************************************************************** + * Name: up_x11createframe + ***************************************************************************/ + +static inline int up_x11createframe(void) +{ + XGCValues gcval; + char *argv[2] = { "nuttx", NULL }; + char *winName = "NuttX"; + char *iconName = "NX"; + XTextProperty winprop; + XTextProperty iconprop; + XSizeHints hints; + + g_display = XOpenDisplay(NULL); + if (g_display == NULL) + { + printf("Unable to open display.\n"); + return -1; + } + + g_screen = DefaultScreen(g_display); + g_window = XCreateSimpleWindow(g_display, DefaultRootWindow(g_display), + 0, 0, g_fbpixelwidth, g_fbpixelheight, 2, + BlackPixel(g_display, g_screen), + BlackPixel(g_display, g_screen)); + + XStringListToTextProperty(&winName, 1, &winprop); + XStringListToTextProperty(&iconName, 1, &iconprop); + + hints.flags = PSize | PMinSize | PMaxSize; + hints.width = hints.min_width = hints.max_width = g_fbpixelwidth; + hints.height= hints.min_height = hints.max_height = g_fbpixelheight; + + XSetWMProperties(g_display, g_window, &winprop, &iconprop, argv, 1, + &hints, NULL, NULL); + + XMapWindow(g_display, g_window); + + /* Select window input events */ + + XSelectInput(g_display, g_window, + ButtonPressMask|ButtonReleaseMask|ButtonMotionMask|KeyPressMask); + + /* Release queued events on the display */ + +#ifdef CONFIG_SIM_TOUCHSCREEN + (void)XAllowEvents(g_display, AsyncBoth, CurrentTime); + + /* Grab mouse button 1, enabling mouse-related events */ + + (void)XGrabButton(g_display, Button1, AnyModifier, g_window, 1, + ButtonPressMask|ButtonReleaseMask|ButtonMotionMask, + GrabModeAsync, GrabModeAsync, None, None); +#endif + + gcval.graphics_exposures = 0; + g_gc = XCreateGC(g_display, g_window, GCGraphicsExposures, &gcval); + + return 0; +} + +/**************************************************************************** + * Name: up_x11errorhandler + ***************************************************************************/ + +#ifndef CONFIG_SIM_X11NOSHM +static int up_x11errorhandler(Display *display, XErrorEvent *event) +{ + g_xerror = 1; + return 0; +} +#endif + +/**************************************************************************** + * Name: up_x11traperrors + ***************************************************************************/ + +#ifndef CONFIG_SIM_X11NOSHM +static void up_x11traperrors(void) +{ + g_xerror = 0; + XSetErrorHandler(up_x11errorhandler); +} +#endif + +/**************************************************************************** + * Name: up_x11untraperrors + ***************************************************************************/ + +#ifndef CONFIG_SIM_X11NOSHM +static int up_x11untraperrors(void) +{ + XSync(g_display,0); + XSetErrorHandler(NULL); + return g_xerror; +} +#endif + +/**************************************************************************** + * Name: up_x11uninitX + ***************************************************************************/ + +static void up_x11uninitX(void) +{ + fprintf(stderr, "Uninitalizing X\n"); + if (g_x11initialized) + { +#ifndef CONFIG_SIM_X11NOSHM + if (g_shmcheckpoint > 4) + { + XShmDetach(g_display, &g_xshminfo); + } + + if (g_shmcheckpoint > 3) + { + shmdt(g_xshminfo.shmaddr); + } + + if (g_shmcheckpoint > 2) + { + shmctl(g_xshminfo.shmid, IPC_RMID, 0); + } +#endif + + if (g_shmcheckpoint > 1) + { + XDestroyImage(g_image); + } + + /* Un-grab the mouse buttons */ + +#ifdef CONFIG_SIM_TOUCHSCREEN + XUngrabButton(g_display, Button1, AnyModifier, g_window); +#endif + g_x11initialized = 0; + } + + XCloseDisplay(g_display); +} + +/**************************************************************************** + * Name: up_x11uninitialize + ***************************************************************************/ + +#ifndef CONFIG_SIM_X11NOSHM +static void up_x11uninitialize(void) +{ + fprintf(stderr, "Uninitalizing\n"); + if (g_shmcheckpoint > 1) + { + if (!b_useshm && g_framebuffer) + { + free(g_framebuffer); + g_framebuffer = 0; + } + } + + if (g_shmcheckpoint > 0) + { + g_shmcheckpoint = 1; + } +} +#endif + +/**************************************************************************** + * Name: up_x11mapsharedmem + ***************************************************************************/ + +static inline int up_x11mapsharedmem(int depth, unsigned int fblen) +{ +#ifndef CONFIG_SIM_X11NOSHM + Status result; +#endif + + atexit(up_x11uninitX); + g_shmcheckpoint = 1; + b_useshm = 0; + +#ifndef CONFIG_SIM_X11NOSHM + if (XShmQueryExtension(g_display)) + { + b_useshm = 1; + printf("Using shared memory.\n"); + + up_x11traperrors(); + g_image = XShmCreateImage(g_display, DefaultVisual(g_display, g_screen), + depth, ZPixmap, NULL, &g_xshminfo, + g_fbpixelwidth, g_fbpixelheight); + if (up_x11untraperrors()) + { + up_x11uninitialize(); + goto shmerror; + } + if (!g_image) + { + fprintf(stderr,"Unable to create g_image."); + return -1; + } + g_shmcheckpoint++; + + g_xshminfo.shmid = shmget(IPC_PRIVATE, + g_image->bytes_per_line * g_image->height, + IPC_CREAT | 0777); + if (g_xshminfo.shmid < 0) + { + up_x11uninitialize(); + goto shmerror; + } + g_shmcheckpoint++; + + g_image->data = (char *) shmat(g_xshminfo.shmid, 0, 0); + if (g_image->data == ((char *) -1)) + { + up_x11uninitialize(); + goto shmerror; + } + g_shmcheckpoint++; + + g_xshminfo.shmaddr = g_image->data; + g_xshminfo.readOnly = 0; + + up_x11traperrors(); + result = XShmAttach(g_display, &g_xshminfo); + if (up_x11untraperrors() || !result) + { + up_x11uninitialize(); + goto shmerror; + } + + g_shmcheckpoint++; + + } + else +#endif + if (!b_useshm) + { +#ifndef CONFIG_SIM_X11NOSHM +shmerror: +#endif + b_useshm = 0; + + g_framebuffer = (unsigned char*)malloc(fblen); + + g_image = XCreateImage(g_display, DefaultVisual(g_display,g_screen), depth, + ZPixmap, 0, (char*)g_framebuffer, g_fbpixelwidth, g_fbpixelheight, + 8, 0); + + if (g_image == NULL) + { + fprintf(stderr, "Unable to create g_image\n"); + return -1; + } + + g_shmcheckpoint++; + } + return 0; +} + +/**************************************************************************** + * Public Functions + ***************************************************************************/ + +/**************************************************************************** + * Name: up_x11initialize + * + * Description: + * Make an X11 window look like a frame buffer. + * + ***************************************************************************/ + +int up_x11initialize(unsigned short width, unsigned short height, + void **fbmem, unsigned int *fblen, unsigned char *bpp, + unsigned short *stride) +{ + XWindowAttributes windowAttributes; + int depth; + int ret; + + /* Check if we are already initialized */ + + if (!g_x11initialized) + { + /* Save inputs */ + + g_fbpixelwidth = width; + g_fbpixelheight = height; + + /* Create the X11 window */ + + ret = up_x11createframe(); + if (ret < 0) + { + return ret; + } + + /* Determine the supported pixel bpp of the current window */ + + XGetWindowAttributes(g_display, DefaultRootWindow(g_display), &windowAttributes); + + /* Get the pixel depth. If the depth is 24-bits, use 32 because X expects + * 32-bit aligment anyway. + */ + + depth = windowAttributes.depth; + if (depth == 24) + { + depth = 32; + } + printf("Pixel bpp is %d bits (using %d)\n", windowAttributes.depth, depth); + + *bpp = depth; + *stride = (depth * width / 8); + *fblen = (*stride * height); + + /* Map the window to shared memory */ + + up_x11mapsharedmem(windowAttributes.depth, *fblen); + g_x11initialized = 1; + } + + *fbmem = (void*)g_framebuffer; + return 0; +} + +/**************************************************************************** + * Name: up_x11cmap + ***************************************************************************/ + +int up_x11cmap(unsigned short first, unsigned short len, + unsigned char *red, unsigned char *green, + unsigned char *blue, unsigned char *transp) +{ + Colormap cMap; + int ndx; + + printf("Creating Colormap\n"); + + /* Convert each color to X11 scaling */ + + cMap = DefaultColormap(g_display, g_screen); + for (ndx = first; ndx < first + len; ndx++) + { + XColor color; + + /* Convert to RGB. In the NuttX cmap, each component + * ranges from 0-255; for X11 the range is 0-65536 */ + + color.red = (short)(*red++) << 8; + color.green = (short)(*green++) << 8; + color.blue = (short)(*blue++) << 8; + color.flags = DoRed | DoGreen | DoBlue; + + /* Then allocate a color for this selection */ + + if (!XAllocColor(g_display, cMap, &color)) + { + fprintf(stderr, "Failed to allocate color%d\n", ndx); + return -1; + } + } + + return 0; +} + +/**************************************************************************** + * Name: up_x11update + ***************************************************************************/ + +void up_x11update(void) +{ +#ifndef CONFIG_SIM_X11NOSHM + if (b_useshm) + { + XShmPutImage(g_display, g_window, g_gc, g_image, 0, 0, 0, 0, + g_fbpixelwidth, g_fbpixelheight, 0); + } + else +#endif + { + XPutImage(g_display, g_window, g_gc, g_image, 0, 0, 0, 0, + g_fbpixelwidth, g_fbpixelheight); + } + XSync(g_display, 0); +} diff --git a/nuttx/configs/Kconfig b/nuttx/configs/Kconfig index ebe6d5480114..9190eab79e89 100644 --- a/nuttx/configs/Kconfig +++ b/nuttx/configs/Kconfig @@ -24,12 +24,22 @@ config ARCH_BOARD_PX4IO ---help--- PX4 system I/O expansion board +config ARCH_BOARD_PX4FMU_SIM + bool "PX4FMU User mode simulation" + depends on ARCH_SIM + ---help--- + A user-mode port of NuttX to the x86 Linux/Cygwin platform is available. + The purpose of this port is primarily to support OS feature development. + This port does not support interrupts or a real timer (and hence no + round robin scheduler) Otherwise, it is complete. + endchoice config ARCH_BOARD string default "px4fmu" if ARCH_BOARD_PX4FMU default "px4io" if ARCH_BOARD_PX4IO + default "px4fmu_sim" if ARCH_BOARD_PX4FMU_SIM default "" if ARCH_BOARD_CUSTOM if ARCH_BOARD_PX4FMU @@ -38,3 +48,6 @@ endif if ARCH_BOARD_PX4IO source "configs/px4io/Kconfig" endif +if ARCH_BOARD_PX4FMU_SIM +source "configs/sim/Kconfig" +endif diff --git a/nuttx/configs/px4fmu_sim/Kconfig b/nuttx/configs/px4fmu_sim/Kconfig new file mode 100644 index 000000000000..b5e6d5515f97 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/Kconfig @@ -0,0 +1,26 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMU +config ARCH_LEDS + bool "NuttX LED support" + default n + ---help--- + "Support control of board LEDs by NuttX to indicate system state" + +config ARCH_BUTTONS + bool "Button support" + default n + ---help--- + "Support interfaces to use buttons provided by the board." + +config ARCH_IRQBUTTONS + bool "Button interrupt support" + default n + depends on ARCH_BUTTONS + ---help--- + "Support EXTI interrupts on button presses and releases." + +endif diff --git a/nuttx/configs/px4fmu_sim/README.txt b/nuttx/configs/px4fmu_sim/README.txt new file mode 100755 index 000000000000..c92169206b23 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/README.txt @@ -0,0 +1,601 @@ +README +====== + +This README discusses issues unique to NuttX configurations for the +PX4FMU development board. + +Or, it will once those are established. For now, this is a copy of the file +as presented for the STMicro STM32F407 evaluation board. Read with caution. + +Contents +======== + + - Development Environment + - GNU Toolchain Options + - IDEs + - NuttX buildroot Toolchain + - STM3240G-EVAL-specific Configuration Options + - LEDs + - Ethernet + - PWM + - CAN + - Configurations + +Development Environment +======================= + + Either Linux or Cygwin on Windows can be used for the development environment. + The source has been built only using the GNU toolchain (see below). Other + toolchains will likely cause problems. Testing was performed using the Cygwin + environment because the Raisonance R-Link emulatator and some RIDE7 development tools + were used and those tools works only under Windows. + +GNU Toolchain Options +===================== + + The NuttX make system has been modified to support the following different + toolchain options. + + 1. The CodeSourcery GNU toolchain, + 2. The devkitARM GNU toolchain, + 3. Raisonance GNU toolchain, or + 4. The NuttX buildroot Toolchain (see below). + + All testing has been conducted using the CodeSourcery toolchain for Windows. To use + the devkitARM, Raisonance GNU, or NuttX buildroot toolchain, you simply need to + add one of the following configuration options to your .config (or defconfig) + file: + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_STM32_CODESOURCERYL=y : CodeSourcery under Linux + CONFIG_STM32_DEVKITARM=y : devkitARM under Windows + CONFIG_STM32_RAISONANCE=y : Raisonance RIDE7 under Windows + CONFIG_STM32_BUILDROOT=y : NuttX buildroot under Linux or Cygwin (default) + + If you change the default toolchain, then you may also have to modify the PATH in + the setenv.h file if your make cannot find the tools. + + NOTE: the CodeSourcery (for Windows), devkitARM, and Raisonance toolchains are + Windows native toolchains. The CodeSourcey (for Linux) and NuttX buildroot + toolchains are Cygwin and/or Linux native toolchains. There are several limitations + to using a Windows based toolchain in a Cygwin environment. The three biggest are: + + 1. The Windows toolchain cannot follow Cygwin paths. Path conversions are + performed automatically in the Cygwin makefiles using the 'cygpath' utility + but you might easily find some new path problems. If so, check out 'cygpath -w' + + 2. Windows toolchains cannot follow Cygwin symbolic links. Many symbolic links + are used in Nuttx (e.g., include/arch). The make system works around these + problems for the Windows tools by copying directories instead of linking them. + But this can also cause some confusion for you: For example, you may edit + a file in a "linked" directory and find that your changes had no effect. + That is because you are building the copy of the file in the "fake" symbolic + directory. If you use a Windows toolchain, you should get in the habit of + making like this: + + make clean_context all + + An alias in your .bashrc file might make that less painful. + + 3. Dependencies are not made when using Windows versions of the GCC. This is + because the dependencies are generated using Windows pathes which do not + work with the Cygwin make. + + Support has been added for making dependencies with the windows-native toolchains. + That support can be enabled by modifying your Make.defs file as follows: + + - MKDEP = $(TOPDIR)/tools/mknulldeps.sh + + MKDEP = $(TOPDIR)/tools/mkdeps.sh --winpaths "$(TOPDIR)" + + If you have problems with the dependency build (for example, if you are not + building on C:), then you may need to modify tools/mkdeps.sh + + NOTE 1: The CodeSourcery toolchain (2009q1) does not work with default optimization + level of -Os (See Make.defs). It will work with -O0, -O1, or -O2, but not with + -Os. + + NOTE 2: The devkitARM toolchain includes a version of MSYS make. Make sure that + the paths to Cygwin's /bin and /usr/bin directories appear BEFORE the devkitARM + path or will get the wrong version of make. + +IDEs +==== + + NuttX is built using command-line make. It can be used with an IDE, but some + effort will be required to create the project. + + Makefile Build + -------------- + Under Eclipse, it is pretty easy to set up an "empty makefile project" and + simply use the NuttX makefile to build the system. That is almost for free + under Linux. Under Windows, you will need to set up the "Cygwin GCC" empty + makefile project in order to work with Windows (Google for "Eclipse Cygwin" - + there is a lot of help on the internet). + + Native Build + ------------ + Here are a few tips before you start that effort: + + 1) Select the toolchain that you will be using in your .config file + 2) Start the NuttX build at least one time from the Cygwin command line + before trying to create your project. This is necessary to create + certain auto-generated files and directories that will be needed. + 3) Set up include pathes: You will need include/, arch/arm/src/stm32, + arch/arm/src/common, arch/arm/src/armv7-m, and sched/. + 4) All assembly files need to have the definition option -D __ASSEMBLY__ + on the command line. + + Startup files will probably cause you some headaches. The NuttX startup file + is arch/arm/src/stm32/stm32_vectors.S. With RIDE, I have to build NuttX + one time from the Cygwin command line in order to obtain the pre-built + startup object needed by RIDE. + +NuttX buildroot Toolchain +========================= + + A GNU GCC-based toolchain is assumed. The files */setenv.sh should + be modified to point to the correct path to the Cortex-M3 GCC toolchain (if + different from the default in your PATH variable). + + If you have no Cortex-M3 toolchain, one can be downloaded from the NuttX + SourceForge download site (https://sourceforge.net/project/showfiles.php?group_id=189573). + This GNU toolchain builds and executes in the Linux or Cygwin environment. + + 1. You must have already configured Nuttx in /nuttx. + + cd tools + ./configure.sh stm3240g-eval/ + + 2. Download the latest buildroot package into + + 3. unpack the buildroot tarball. The resulting directory may + have versioning information on it like buildroot-x.y.z. If so, + rename /buildroot-x.y.z to /buildroot. + + 4. cd /buildroot + + 5. cp configs/cortexm3-defconfig-4.3.3 .config + + 6. make oldconfig + + 7. make + + 8. Edit setenv.h, if necessary, so that the PATH variable includes + the path to the newly built binaries. + + See the file configs/README.txt in the buildroot source tree. That has more + detailed PLUS some special instructions that you will need to follow if you are + building a Cortex-M3 toolchain for Cygwin under Windows. + +Ethernet +======== + +The Ethernet driver is configured to use the MII interface: + + Board Jumper Settings: + + Jumper Description + JP8 To enable MII, JP8 should not be fitted. + JP6 2-3: Enable MII interface mode + JP5 2-3: Provide 25 MHz clock for MII or 50 MHz clock for RMII by MCO at PA8 + SB1 Not used with MII + +LEDs +==== + +The STM3240G-EVAL board has four LEDs labeled LD1, LD2, LD3 and LD4 on the +board.. These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is +defined. In that case, the usage by the board port is defined in +include/board.h and src/up_leds.c. The LEDs are used to encode OS-related\ +events as follows: + + SYMBOL Meaning LED1* LED2 LED3 LED4 + ------------------- ----------------------- ------- ------- ------- ------ + LED_STARTED NuttX has been started ON OFF OFF OFF + LED_HEAPALLOCATE Heap has been allocated OFF ON OFF OFF + LED_IRQSENABLED Interrupts enabled ON ON OFF OFF + LED_STACKCREATED Idle stack created OFF OFF ON OFF + LED_INIRQ In an interrupt** ON N/C N/C OFF + LED_SIGNAL In a signal handler*** N/C ON N/C OFF + LED_ASSERTION An assertion failed ON ON N/C OFF + LED_PANIC The system has crashed N/C N/C N/C ON + LED_IDLE STM32 is is sleep mode (Optional, not used) + + * If LED1, LED2, LED3 are statically on, then NuttX probably failed to boot + and these LEDs will give you some indication of where the failure was + ** The normal state is LED3 ON and LED1 faintly glowing. This faint glow + is because of timer interupts that result in the LED being illuminated + on a small proportion of the time. +*** LED2 may also flicker normally if signals are processed. + +PWM +=== + +The STM3240G-Eval has no real on-board PWM devices, but the board can be +configured to output a pulse train using TIM4 CH2. This pin is used by +FSMC is but is also connected to the Motor Control Connector (CN5) just +for this purpose: + + PD13 FSMC_A18 / MC_TIM4_CH2 pin 33 (EnB) + +FSMC must be disabled in this case! PD13 is available at: + + Daughterboard Extension Connector, CN3, pin 32 - available + TFT LCD Connector, CN19, pin 17 -- not available without removing the LCD. + Motor Control Connector CN15, pin 33 -- not available unless you bridge SB14. + +CAN +=== + +Connector 10 (CN10) is DB-9 male connector that can be used with CAN1 or CAN2. + + JP10 connects CAN1_RX or CAN2_RX to the CAN transceiver + JP3 connects CAN1_TX or CAN2_TX to the CAN transceiver + +CAN signals are then available on CN10 pins: + + CN10 Pin 7 = CANH + CN10 Pin 2 = CANL + +Mapping to STM32 GPIO pins: + + PD0 = FSMC_D2 & CAN1_RX + PD1 = FSMC_D3 & CAN1_TX + PB13 = ULPI_D6 & CAN2_TX + PB5 = ULPI_D7 & CAN2_RX + +Configuration Options: + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + + CONFIG_STM32_CAN1 - Enable support for CAN1 + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_STM32_CAN2 - Enable support for CAN1 + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + +STM3240G-EVAL-specific Configuration Options +============================================ + + CONFIG_ARCH - Identifies the arch/ subdirectory. This should + be set to: + + CONFIG_ARCH=arm + + CONFIG_ARCH_family - For use in C code: + + CONFIG_ARCH_ARM=y + + CONFIG_ARCH_architecture - For use in C code: + + CONFIG_ARCH_CORTEXM4=y + + CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory + + CONFIG_ARCH_CHIP=stm32 + + CONFIG_ARCH_CHIP_name - For use in C code to identify the exact + chip: + + CONFIG_ARCH_CHIP_STM32F407IG=y + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG - Enables special STM32 clock + configuration features. + + CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG=n + + CONFIG_ARCH_BOARD - Identifies the configs subdirectory and + hence, the board that supports the particular chip or SoC. + + CONFIG_ARCH_BOARD=stm3240g_eval (for the STM3240G-EVAL development board) + + CONFIG_ARCH_BOARD_name - For use in C code + + CONFIG_ARCH_BOARD_STM3240G_EVAL=y + + CONFIG_ARCH_LOOPSPERMSEC - Must be calibrated for correct operation + of delay loops + + CONFIG_ENDIAN_BIG - define if big endian (default is little + endian) + + CONFIG_DRAM_SIZE - Describes the installed DRAM (SRAM in this case): + + CONFIG_DRAM_SIZE=0x00010000 (64Kb) + + CONFIG_DRAM_START - The start address of installed DRAM + + CONFIG_DRAM_START=0x20000000 + + CONFIG_DRAM_END - Last address+1 of installed RAM + + CONFIG_DRAM_END=(CONFIG_DRAM_START+CONFIG_DRAM_SIZE) + + CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization + + CONFIG_ARCH_IRQPRIO=y + + CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU) + + CONFIG_ARCH_FPU=y + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to boards that + have LEDs + + CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt + stack. If defined, this symbol is the size of the interrupt + stack in bytes. If not defined, the user task stacks will be + used during interrupt handling. + + CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions + + CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. + + CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that + cause a 100 second delay during boot-up. This 100 second delay + serves no purpose other than it allows you to calibratre + CONFIG_ARCH_LOOPSPERMSEC. You simply use a stop watch to measure + the 100 second delay then adjust CONFIG_ARCH_LOOPSPERMSEC until + the delay actually is 100 seconds. + + Individual subsystems can be enabled: + + AHB1 + ---- + CONFIG_STM32_CRC + CONFIG_STM32_BKPSRAM + CONFIG_STM32_CCMDATARAM + CONFIG_STM32_DMA1 + CONFIG_STM32_DMA2 + CONFIG_STM32_ETHMAC + CONFIG_STM32_OTGHS + + AHB2 + ---- + CONFIG_STM32_DCMI + CONFIG_STM32_CRYP + CONFIG_STM32_HASH + CONFIG_STM32_RNG + CONFIG_STM32_OTGFS + + AHB3 + ---- + CONFIG_STM32_FSMC + + APB1 + ---- + CONFIG_STM32_TIM2 + CONFIG_STM32_TIM3 + CONFIG_STM32_TIM4 + CONFIG_STM32_TIM5 + CONFIG_STM32_TIM6 + CONFIG_STM32_TIM7 + CONFIG_STM32_TIM12 + CONFIG_STM32_TIM13 + CONFIG_STM32_TIM14 + CONFIG_STM32_WWDG + CONFIG_STM32_SPI2 + CONFIG_STM32_SPI3 + CONFIG_STM32_USART2 + CONFIG_STM32_USART3 + CONFIG_STM32_UART4 + CONFIG_STM32_UART5 + CONFIG_STM32_I2C1 + CONFIG_STM32_I2C2 + CONFIG_STM32_I2C3 + CONFIG_STM32_CAN1 + CONFIG_STM32_CAN2 + CONFIG_STM32_DAC1 + CONFIG_STM32_DAC2 + CONFIG_STM32_PWR -- Required for RTC + + APB2 + ---- + CONFIG_STM32_TIM1 + CONFIG_STM32_TIM8 + CONFIG_STM32_USART1 + CONFIG_STM32_USART6 + CONFIG_STM32_ADC1 + CONFIG_STM32_ADC2 + CONFIG_STM32_ADC3 + CONFIG_STM32_SDIO + CONFIG_STM32_SPI1 + CONFIG_STM32_SYSCFG + CONFIG_STM32_TIM9 + CONFIG_STM32_TIM10 + CONFIG_STM32_TIM11 + + Timer and I2C devices may need to the following to force power to be applied + unconditionally at power up. (Otherwise, the device is powered when it is + initialized). + + CONFIG_STM32_FORCEPOWER + + Timer devices may be used for different purposes. One special purpose is + to generate modulated outputs for such things as motor control. If CONFIG_STM32_TIMn + is defined (as above) then the following may also be defined to indicate that + the timer is intended to be used for pulsed output modulation, ADC conversion, + or DAC conversion. + + CONFIG_STM32_TIM1_PWM + CONFIG_STM32_TIM2_PWM + CONFIG_STM32_TIM3_PWM + CONFIG_STM32_TIM4_PWM + CONFIG_STM32_TIM5_PWM + CONFIG_STM32_TIM8_PWM + CONFIG_STM32_TIM9_PWM + CONFIG_STM32_TIM10_PWM + CONFIG_STM32_TIM11_PWM + CONFIG_STM32_TIM12_PWM + CONFIG_STM32_TIM13_PWM + CONFIG_STM32_TIM14_PWM + + CONFIG_STM32_TIM1_ADC + CONFIG_STM32_TIM2_ADC + CONFIG_STM32_TIM3_ADC + CONFIG_STM32_TIM4_ADC + CONFIG_STM32_TIM5_ADC + CONFIG_STM32_TIM6_ADC + CONFIG_STM32_TIM7_ADC + CONFIG_STM32_TIM8_ADC + + CONFIG_STM32_TIM1_DAC + CONFIG_STM32_TIM2_DAC + CONFIG_STM32_TIM3_DAC + CONFIG_STM32_TIM4_DAC + CONFIG_STM32_TIM5_DAC + CONFIG_STM32_TIM6_DAC + CONFIG_STM32_TIM7_DAC + CONFIG_STM32_TIM8_DAC + + For each timer that is enabled for PWM usage, we need the following additional + configuration settings: + + CONFIG_STM32_TIMx_CHANNEL - Specifies the timer output channel {1,..,4} + + NOTE: The STM32 timers are each capable of generating different signals on + each of the four channels with different duty cycles. That capability is + not supported by this driver: Only one output channel per timer. + + JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): + + CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) + but without JNTRST. + CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled + + STM3240xxx specific device driver settings + + CONFIG_U[S]ARTn_SERIAL_CONSOLE - selects the USARTn (n=1,2,3) or UART + m (m=4,5) for the console and ttys0 (default is the USART1). + CONFIG_U[S]ARTn_RXBUFSIZE - Characters are buffered as received. + This specific the size of the receive buffer + CONFIG_U[S]ARTn_TXBUFSIZE - Characters are buffered before + being sent. This specific the size of the transmit buffer + CONFIG_U[S]ARTn_BAUD - The configure BAUD of the UART. Must be + CONFIG_U[S]ARTn_BITS - The number of bits. Must be either 7 or 8. + CONFIG_U[S]ARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity + CONFIG_U[S]ARTn_2STOP - Two stop bits + + CONFIG_STM32_SPI_INTERRUPTS - Select to enable interrupt driven SPI + support. Non-interrupt-driven, poll-waiting is recommended if the + interrupt rate would be to high in the interrupt driven case. + CONFIG_STM32_SPI_DMA - Use DMA to improve SPI transfer performance. + Cannot be used with CONFIG_STM32_SPI_INTERRUPT. + + CONFIG_SDIO_DMA - Support DMA data transfers. Requires CONFIG_STM32_SDIO + and CONFIG_STM32_DMA2. + CONFIG_SDIO_PRI - Select SDIO interrupt prority. Default: 128 + CONFIG_SDIO_DMAPRIO - Select SDIO DMA interrupt priority. + Default: Medium + CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: + 4-bit transfer mode. + + CONFIG_STM32_PHYADDR - The 5-bit address of the PHY on the board + CONFIG_STM32_MII - Support Ethernet MII interface + CONFIG_STM32_MII_MCO1 - Use MCO1 to clock the MII interface + CONFIG_STM32_MII_MCO2 - Use MCO2 to clock the MII interface + CONFIG_STM32_RMII - Support Ethernet RMII interface + CONFIG_STM32_AUTONEG - Use PHY autonegotion to determine speed and mode + CONFIG_STM32_ETHFD - If CONFIG_STM32_AUTONEG is not defined, then this + may be defined to select full duplex mode. Default: half-duplex + CONFIG_STM32_ETH100MBPS - If CONFIG_STM32_AUTONEG is not defined, then this + may be defined to select 100 MBps speed. Default: 10 Mbps + CONFIG_STM32_PHYSR - This must be provided if CONFIG_STM32_AUTONEG is + defined. The PHY status register address may diff from PHY to PHY. This + configuration sets the address of the PHY status register. + CONFIG_STM32_PHYSR_SPEED - This must be provided if CONFIG_STM32_AUTONEG is + defined. This provides bit mask indicating 10 or 100MBps speed. + CONFIG_STM32_PHYSR_100MBPS - This must be provided if CONFIG_STM32_AUTONEG is + defined. This provides the value of the speed bit(s) indicating 100MBps speed. + CONFIG_STM32_PHYSR_MODE - This must be provided if CONFIG_STM32_AUTONEG is + defined. This provide bit mask indicating full or half duplex modes. + CONFIG_STM32_PHYSR_FULLDUPLEX - This must be provided if CONFIG_STM32_AUTONEG is + defined. This provides the value of the mode bits indicating full duplex mode. + CONFIG_STM32_ETH_PTP - Precision Time Protocol (PTP). Not supported + but some hooks are indicated with this condition. + + STM3240G-EVAL CAN Configuration + + CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or + CONFIG_STM32_CAN2 must also be defined) + CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. + Default: 8 + CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. + Default: 4 + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. + CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. + CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an + dump of all CAN registers. + + STM3240G-EVAL LCD Hardware Configuration + +Configurations +============== + +Each STM3240G-EVAL configuration is maintained in a sudirectory and +can be selected as follow: + + cd tools + ./configure.sh stm3240g-eval/ + cd - + . ./setenv.sh + +Where is one of the following: + + dhcpd: + ----- + + This builds the DCHP server using the apps/examples/dhcpd application + (for execution from FLASH.) See apps/examples/README.txt for information + about the dhcpd example. The server address is 10.0.0.1 and it serves + IP addresses in the range 10.0.0.2 through 10.0.0.17 (all of which, of + course, are configurable). + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + nettest: + ------- + + This configuration directory may be used to verify networking performance + using the STM32's Ethernet controller. It uses apps/examples/nettest to excercise the + TCP/IP network. + + CONFIG_EXAMPLE_NETTEST_SERVER=n : Target is configured as the client + CONFIG_EXAMPLE_NETTEST_PERFORMANCE=y : Only network performance is verified. + CONFIG_EXAMPLE_NETTEST_IPADDR=(10<<24|0<<16|0<<8|2) : Target side is IP: 10.0.0.2 + CONFIG_EXAMPLE_NETTEST_DRIPADDR=(10<<24|0<<16|0<<8|1) : Host side is IP: 10.0.0.1 + CONFIG_EXAMPLE_NETTEST_CLIENTIP=(10<<24|0<<16|0<<8|1) : Server address used by which ever is client. + + ostest: + ------ + This configuration directory, performs a simple OS test using + examples/ostest. By default, this project assumes that you are + using the DFU bootloader. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + + nsh: + --- + Configures the NuttShell (nsh) located at apps/examples/nsh. The + Configuration enables both the serial and telnet NSH interfaces. + + CONFIG_STM32_CODESOURCERYW=y : CodeSourcery under Windows + CONFIG_NSH_DHCPC=n : DHCP is disabled + CONFIG_NSH_IPADDR=(10<<24|0<<16|0<<8|2) : Target IP address 10.0.0.2 + CONFIG_NSH_DRIPADDR=(10<<24|0<<16|0<<8|1) : Host IP address 10.0.0.1 + + NOTE: This example assumes that a network is connected. During its + initialization, it will try to negotiate the link speed. If you have + no network connected when you reset the board, there will be a long + delay (maybe 30 seconds?) before anything happens. That is the timeout + before the networking finally gives up and decides that no network is + available. diff --git a/nuttx/configs/px4fmu_sim/common/Make.defs b/nuttx/configs/px4fmu_sim/common/Make.defs new file mode 100644 index 000000000000..7987fcb3630a --- /dev/null +++ b/nuttx/configs/px4fmu_sim/common/Make.defs @@ -0,0 +1,200 @@ +############################################################################ +# configs/sim/nsh/Make.defs +# +# Copyright (C) 2008, 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +HOSTOS = ${shell uname -o 2>/dev/null || echo "Other"} +Q= + +CROSSDEV = + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/common/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + + + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + $(ARCHSCRIPT) #\ + #--gc-sections +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +define PREPROCESS + @echo "CPP: $1->$2" + $(Q)$(CPP) $(CPPFLAGS) $(abspath $1) -o $2 +endef + +define COMPILE + @echo "CC: $1" + $(Q)$(CC) -c $(CFLAGS) $(abspath $1) -o $2 +endef + +define COMPILEXX + @echo "CXX: $1" + $(Q)$(CXX) -c $(CXXFLAGS) $(abspath $1) -o $2 +endef + +define ASSEMBLE + @echo "AS: $1" + $(Q)$(CC) -c $(AFLAGS) $(abspath $1) -o $2 +endef + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q)$(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +define ARCHIVE + echo "AR: $2"; \ + $(AR) $1 $2 || { echo "$(AR) $1 $2 FAILED!" ; exit 1 ; } +endef + +define CLEAN + $(Q)rm -f *.o *.a +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx/configs/px4fmu_sim/common/ld.script b/nuttx/configs/px4fmu_sim/common/ld.script new file mode 100644 index 000000000000..9ae421151b16 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/common/ld.script @@ -0,0 +1,149 @@ +/**************************************************************************** + * configs/px4fmu/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +/* The STM32F405 has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x00000000, LENGTH = 10M + sram (rwx) : ORIGIN = 0x10000000, LENGTH = 10M + ccsram (rwx) : ORIGIN = 0x20000000, LENGTH = 10M +} + +/*OUTPUT_ARCH(arm)*/ + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = .; + KEEP(*(__param*)) + __param_end = .; + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx/configs/px4fmu_sim/include/board.h b/nuttx/configs/px4fmu_sim/include/board.h new file mode 100755 index 000000000000..3f0f26ba144d --- /dev/null +++ b/nuttx/configs/px4fmu_sim/include/board.h @@ -0,0 +1,366 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMU uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* High-resolution timer + */ +#ifdef CONFIG_HRT_TIMER +# define HRT_TIMER 1 /* use timer1 for the HRT */ +# define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#endif + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + * + * Note that UART5 has no optional pinout. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 +#define GPIO_USART2_RTS GPIO_USART2_RTS_1 +#define GPIO_USART2_CTS GPIO_USART2_CTS_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* UART DMA configuration for USART1/6 */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * PWM + * + * Four PWM outputs can be configured on pins otherwise shared with + * USART2; two can take the flow control pins if they are not being used. + * + * Pins: + * + * CTS - PA0 - TIM2CH1 + * RTS - PA1 - TIM2CH2 + * TX - PA2 - TIM2CH3 + * RX - PA3 - TIM2CH4 + * + */ +#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1 +#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 +#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 +#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 + +/* + * PPM + * + * PPM input is handled by the HRT timer. + */ +#if defined(CONFIG_HRT_TIMER) && defined (CONFIG_HRT_PPM) +# define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +# define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +#endif + +/* + * CAN + * + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_2 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_MS5611 0x76 +#define PX4_I2C_OBDEV_EEPROM NOTDEFINED + +#define PX4_I2C_OBDEV_PX4IO_BL 0x18 +#define PX4_I2C_OBDEV_PX4IO 0x19 + +/* + * SPI + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 +#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 + +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL 2 +#define PX4_SPIDEV_MPU 3 + +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 3 /* timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4fmu_sim/nsh/Make.defs b/nuttx/configs/px4fmu_sim/nsh/Make.defs new file mode 100644 index 000000000000..9ff26c93ace4 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/nsh/Make.defs @@ -0,0 +1,3 @@ +include ${TOPDIR}/.config + +include $(TOPDIR)/configs/px4fmu_sim/common/Make.defs diff --git a/nuttx/configs/px4fmu_sim/nsh/appconfig b/nuttx/configs/px4fmu_sim/nsh/appconfig new file mode 100644 index 000000000000..a261cb0cf5ab --- /dev/null +++ b/nuttx/configs/px4fmu_sim/nsh/appconfig @@ -0,0 +1,119 @@ +############################################################################ +# configs/px4fmu_sim/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/mqueue_test +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +# System library - utility functions +CONFIGURED_APPS += systemlib +CONFIGURED_APPS += systemlib/mixer + +# System utility commands +CONFIGURED_APPS += systemcmds/reboot +CONFIGURED_APPS += systemcmds/perf +CONFIGURED_APPS += systemcmds/top +#CONFIGURED_APPS += systemcmds/boardinfo +CONFIGURED_APPS += systemcmds/mixer +#CONFIGURED_APPS += systemcmds/eeprom +CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/bl_update +#CONFIGURED_APPS += systemcmds/calibration +CONFIGURED_APPS += systemcmds/uname + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +CONFIGURED_APPS += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/deamon +# CONFIGURED_APPS += examples/px4_deamon_app + +# Shared object broker; required by many parts of the system. +CONFIGURED_APPS += uORB + +CONFIGURED_APPS += mavlink +CONFIGURED_APPS += gps +CONFIGURED_APPS += commander +CONFIGURED_APPS += sdlog +CONFIGURED_APPS += sensors +CONFIGURED_APPS += ardrone_interface +CONFIGURED_APPS += multirotor_att_control +CONFIGURED_APPS += multirotor_pos_control +CONFIGURED_APPS += fixedwing_control +CONFIGURED_APPS += position_estimator +CONFIGURED_APPS += attitude_estimator_ekf + +# Hacking tools +#CONFIGURED_APPS += system/i2c +#CONFIGURED_APPS += tools/i2c_dev + +# Communication and Drivers +#CONFIGURED_APPS += drivers/boards/px4fmu +CONFIGURED_APPS += drivers/device +#CONFIGURED_APPS += drivers/ms5611 +#CONFIGURED_APPS += drivers/hmc5883 +#CONFIGURED_APPS += drivers/mpu6000 +#CONFIGURED_APPS += drivers/bma180 +#CONFIGURED_APPS += drivers/l3gd20 +#CONFIGURED_APPS += drivers/px4io +#CONFIGURED_APPS += drivers/stm32 +#CONFIGURED_APPS += drivers/led +#CONFIGURED_APPS += drivers/stm32/tone_alarm +#CONFIGURED_APPS += drivers/px4fmu + +# Sim +CONFIGURED_APPS += sim/sensors +CONFIGURED_APPS += sim/drivers +CONFIGURED_APPS += sim/drivers/tone_alarm + +# Testing stuff +#CONFIGURED_APPS += px4/sensors_bringup +#CONFIGURED_APPS += px4/tests + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +#CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx/configs/px4fmu_sim/nsh/defconfig b/nuttx/configs/px4fmu_sim/nsh/defconfig new file mode 100755 index 000000000000..94e9bb0f3c31 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/nsh/defconfig @@ -0,0 +1,1048 @@ +############################################################################ +# configs/px4fmu/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The STM3240xxx supports interrupt prioritization +# CONFIG_ARCH_FPU - The STM3240xxx supports a floating point unit (FPU). +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="sim" # SIM CHANGE +CONFIG_ARCH_SIM=y # SIM CHANGE +#CONFIG_ARCH_CORTEXM4=y #SIM CHANGE +#CONFIG_ARCH_CHIP="stm32" # SIM CHANGE +#CONFIG_ARCH_CHIP_STM32F405RG=y # SIM CHANGE +CONFIG_ARCH_BOARD="px4fmu_sim" # SIM CHANGE +CONFIG_ARCH_BOARD_PX4FMU_SIM=y +CONFIG_ARCH_BOARD_PX4FMU=y # SIM CHANGE +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DRAM_SIZE=0x00030000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=y +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y +CONFIG_PM=y # SIM CHANGE allows initialization hook required for sched instru. + +CONFIG_ARMV7M_CMNVECTOR=y + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are enabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG (ignored) +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# On-chip CCM SRAM configuration +# +# CONFIG_STM32_CCMEXCLUDE - Exclude CCM SRAM from the HEAP. You would need +# to do this if DMA is enabled to prevent non-DMA-able CCM memory from +# being a part of the stack. +# + +# +# On-board FSMC SRAM configuration +# +# CONFIG_STM32_FSMC - Required. See below +# CONFIG_MM_REGIONS - Required. Must be 2 or 3 (see above) +# +# CONFIG_STM32_FSMC_SRAM=y - Indicates that SRAM is available via the +# FSMC (as opposed to an LCD or FLASH). +# CONFIG_HEAP2_BASE - The base address of the SRAM in the FSMC address space +# CONFIG_HEAP2_END - The end (+1) of the SRAM in the FSMC address space +# +#CONFIG_STM32_FSMC_SRAM=n +#CONFIG_HEAP2_BASE=0x64000000 +#CONFIG_HEAP2_END=(0x64000000+(2*1024*1024)) + +# +# Individual subsystems can be enabled: +# +# This set is exhaustive for PX4FMU and should be safe to cut and +# paste into any other config. +# +# AHB1: +CONFIG_STM32_CRC=n +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_ETHMAC=n +CONFIG_STM32_OTGHS=n +# AHB2: +CONFIG_STM32_DCMI=n +CONFIG_STM32_CRYP=n +CONFIG_STM32_HASH=n +CONFIG_STM32_RNG=n +CONFIG_STM32_OTGFS=y +# AHB3: +CONFIG_STM32_FSMC=n +# APB1: +# TIM2 is owned by PWM output +CONFIG_STM32_TIM2=n +# TIM3 is owned by TONE_ALARM +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_TIM6=y +CONFIG_STM32_TIM7=y +CONFIG_STM32_TIM12=y +CONFIG_STM32_TIM13=y +CONFIG_STM32_TIM14=y +CONFIG_STM32_WWDG=y +CONFIG_STM32_IWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_SPI3=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=n +CONFIG_STM32_UART4=n +CONFIG_STM32_UART5=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=y +CONFIG_STM32_CAN1=n +CONFIG_STM32_CAN2=n +CONFIG_STM32_DAC=n +CONFIG_STM32_PWR=y +# APB2: +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +# TIM8 is owned by PWM output +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_USART6=y +CONFIG_STM32_ADC1=n +CONFIG_STM32_ADC2=n +CONFIG_STM32_ADC3=y +CONFIG_STM32_SDIO=n +CONFIG_STM32_SPI1=y +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y + + +# +# Enable the MTD driver for the onboard I2C EEPROM +# +# Note that we are using a private copy of the AT24XX driver that +# does not require CONFIG_MTD_XT24XX to be set. +# +#CONFIG_MTD_AT24XX=y +CONFIG_AT24XX_ADDR=0x50 +CONFIG_AT24XX_SIZE=128 +CONFIG_AT24XX_MTD_BLOCKSIZE=256 + +# +# STM32F40xxx specific serial device driver settings +# +# CONFIG_SERIAL_TERMIOS - Serial driver supports termios.h interfaces (tcsetattr, +# tcflush, etc.). If this is not defined, then the terminal settings (baud, +# parity, etc.) are not configurable at runtime; serial streams cannot be +# flushed, etc. +# CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port +# immediately after creating the /dev/console device. This is required +# if the console serial port has RX DMA enabled. +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_SERIAL_CONSOLE_REINIT=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n +CONFIG_UART4_SERIAL_CONSOLE=n +CONFIG_UART5_SERIAL_CONSOLE=n +CONFIG_USART6_SERIAL_CONSOLE=n + +#Mavlink messages can be bigger than 128 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=128 +CONFIG_USART3_TXBUFSIZE=128 +CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART5_TXBUFSIZE=64 +CONFIG_USART6_TXBUFSIZE=128 + +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=128 +CONFIG_USART3_RXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=128 +CONFIG_UART5_RXBUFSIZE=128 +CONFIG_USART6_RXBUFSIZE=128 + +CONFIG_USART1_BAUD=57600 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 +CONFIG_UART4_BAUD=115200 +CONFIG_UART5_BAUD=115200 +CONFIG_USART6_BAUD=9600 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 +CONFIG_UART4_BITS=8 +CONFIG_UART5_BITS=8 +CONFIG_USART6_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 +CONFIG_UART4_PARITY=0 +CONFIG_UART5_PARITY=0 +CONFIG_USART6_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 +CONFIG_UART4_2STOP=0 +CONFIG_UART5_2STOP=0 +CONFIG_USART6_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +CONFIG_USART2_RXDMA=y +CONFIG_USART3_RXDMA=n +CONFIG_UART4_RXDMA=n +CONFIG_UART5_RXDMA=y +CONFIG_USART6_RXDMA=y + +# +# PX4FMU specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_TONE_ALARM +# Enables the tone alarm (buzzer) driver The board definition must +# set TONE_ALARM_TIMER and TONE_ALARM_CHANNEL to the timer and +# capture/compare channels to be used. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# CONFIG_MULTIPORT +# Enabled support for run-time (or EEPROM based boot-time) configuration +# of ports for different functions (e.g. USART2 or ARDrone or PWM out) +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=y +CONFIG_TONE_ALARM=y +CONFIG_PWM_SERVO=n +CONFIG_MULTIPORT=n + +# +# STM32F40xxx specific SPI device driver settings +# +CONFIG_SPI_EXCHANGE=y +# DMA needs more work, not implemented on STM32F4x yet +#CONFIG_STM32_SPI_DMA=y + +# +# STM32F40xxx specific CAN device driver settings +# +# CONFIG_CAN - Enables CAN support (one or both of CONFIG_STM32_CAN1 or +# CONFIG_STM32_CAN2 must also be defined) +# CONFIG_CAN_EXTID - Enables support for the 29-bit extended ID. Default +# Standard 11-bit IDs. +# CONFIG_CAN_FIFOSIZE - The size of the circular buffer of CAN messages. +# Default: 8 +# CONFIG_CAN_NPENDINGRTR - The size of the list of pending RTR requests. +# Default: 4 +# CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback +# mode for testing. The STM32 CAN driver does support loopback mode. +# CONFIG_CAN1_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN1 is defined. +# CONFIG_CAN2_BAUD - CAN1 BAUD rate. Required if CONFIG_STM32_CAN2 is defined. +# CONFIG_CAN_TSEG1 - The number of CAN time quanta in segment 1. Default: 6 +# CONFIG_CAN_TSEG2 - the number of CAN time quanta in segment 2. Default: 7 +# +CONFIG_CAN=n +CONFIG_CAN_EXTID=n +#CONFIG_CAN_FIFOSIZE +#CONFIG_CAN_NPENDINGRTR +CONFIG_CAN_LOOPBACK=n +CONFIG_CAN1_BAUD=700000 +CONFIG_CAN2_BAUD=700000 + +# +# I2C configuration +# +CONFIG_I2C=y +CONFIG_I2C_POLLED=y +CONFIG_I2C_TRANSFER=y +CONFIG_I2C_TRACE=n +# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using +# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update +CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 +# Constant overhead for generating I2C start / stop conditions +CONFIG_STM32_I2CTIMEOUS_START_STOP=700 +# XXX this is bad and we want it gone +CONFIG_I2C_WRITEREAD=y + +# +# ADC configuration +# +# Enable ADC driver support. +# +# CONFIG_ADC=y : Enable the generic ADC infrastructure +# CONFIG_STM32_TIM1_ADC=y : Indicate that timer 1 will be used to trigger an ADC +# CONFIG_STM32_TIM1_ADC3=y : Assign timer 1 to drive ADC3 sampling +# CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=100 : Select a sampling frequency +# +CONFIG_ADC=y +CONFIG_STM32_TIM4_ADC3=y +# select sample frequency 1^=1.5Msamples/second +# 65535^=10samples/second 16bit-timer runs at 84/128 MHz +CONFIG_STM32_ADC3_SAMPLE_FREQUENCY=6000 +# select timer channel 0=CC1,...,3=CC4 +CONFIG_STM32_ADC3_TIMTRIG=3 +CONFIG_ADC_DMA=y +# only 4 places usable! +CONFIG_ADC_FIFOSIZE=5 + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=y + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 192 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# +# CONFIG_SCHED_LPWORK. If CONFIG_SCHED_WORKQUEUE is defined, then a single +# work queue is created by default. If CONFIG_SCHED_LPWORK is also defined +# then an additional, lower-priority work queue will also be created. This +# lower priority work queue is better suited for more extended processing +# (such as file system clean-up operations) +# CONFIG_SCHED_LPWORKPRIORITY - The execution priority of the lower priority +# worker thread. Default: 50 +# CONFIG_SCHED_LPWORKPERIOD - How often the lower priority worker thread +# checks for work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_LPWORKSTACKSIZE - The stack size allocated for the lower +# priority worker thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="nsh_main" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=y +CONFIG_DEBUG_VERBOSE=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=n # SIM CHANGE +CONFIG_MM_REGIONS=2 +CONFIG_ARCH_LOWPUTC=y +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y # SIM CHANGE +CONFIG_TASK_NAME_SIZE=24 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +CONFIG_FDCLONE_DISABLE=n +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=y +CONFIG_SCHED_ATEXIT=y + +# +# System Logging +# +# CONFIG_SYSLOG - Enables the System Logging feature. +# CONFIG_RAMLOG - Enables the RAM logging feature +# CONFIG_RAMLOG_CONSOLE - Use the RAM logging device as a system console. +# If this feature is enabled (along with CONFIG_DEV_CONSOLE), then all +# console output will be re-directed to a circular buffer in RAM. This +# is useful, for example, if the only console is a Telnet console. Then +# in that case, console output from non-Telnet threads will go to the +# circular buffer and can be viewed using the NSH 'dmesg' command. +# CONFIG_RAMLOG_SYSLOG - Use the RAM logging device for the syslogging +# interface. If this feature is enabled (along with CONFIG_SYSLOG), +# then all debug output (only) will be re-directed to the circular +# buffer in RAM. This RAM log can be view from NSH using the 'dmesg' +# command. +# CONFIG_RAMLOG_NPOLLWAITERS - The number of threads than can be waiting +# for this driver on poll(). Default: 4 +# +# If CONFIG_RAMLOG_CONSOLE or CONFIG_RAMLOG_SYSLOG is selected, then the +# following may also be provided: +# +# CONFIG_RAMLOG_CONSOLE_BUFSIZE - Size of the console RAM log. Default: 1024 +# + +CONFIG_SYSLOG=n +CONFIG_RAMLOG=n +CONFIG_RAMLOG_CONSOLE=n +CONFIG_RAMLOG_SYSLOG=n +#CONFIG_RAMLOG_NPOLLWAITERS +#CONFIG_RAMLOG_CONSOLE_BUFSIZE + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=n +CONFIG_DISABLE_PTHREAD=n +CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_MQUEUE=n +CONFIG_DISABLE_MOUNTPOINT=n +CONFIG_DISABLE_ENVIRON=n +CONFIG_DISABLE_POLL=n + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# CONFIG_LIBC_FLOATINGPOINT - Enables printf("%f") +# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing: +# 5.1234567 +# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu) +# +CONFIG_NOPRINTF_FIELDWIDTH=n +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_HAVE_LONG_LONG=y + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=y +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=8 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=25 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=20 +CONFIG_MQ_MAXMSGSIZE=1024 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Filesystem configuration +# +# CONFIG_FS_FAT - Enable FAT filesystem support +# CONFIG_FAT_SECTORSIZE - Max supported sector size +# CONFIG_FAT_LCNAMES - Enable use of the NT-style upper/lower case 8.3 +# file name support. +# CONFIG_FAT_LFN - Enable FAT long file names. NOTE: Microsoft claims +# patents on FAT long file name technology. Please read the +# disclaimer in the top-level COPYING file and only enable this +# feature if you understand these issues. +# CONFIG_FAT_MAXFNAME - If CONFIG_FAT_LFN is defined, then the +# default, maximum long file name is 255 bytes. This can eat up +# a lot of memory (especially stack space). If you are willing +# to live with some non-standard, short long file names, then +# define this value. A good choice would be the same value as +# selected for CONFIG_NAME_MAX which will limit the visibility +# of longer file names anyway. +# CONFIG_FS_NXFFS: Enable NuttX FLASH file system (NXFF) support. +# CONFIG_NXFFS_ERASEDSTATE: The erased state of FLASH. +# This must have one of the values of 0xff or 0x00. +# Default: 0xff. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# CONFIG_NXFFS_MAXNAMLEN: The maximum size of an NXFFS file name. +# Default: 255. +# CONFIG_NXFFS_PACKTHRESHOLD: When packing flash file data, +# don't both with file chunks smaller than this number of data bytes. +# Default: 32. +# CONFIG_NXFFS_TAILTHRESHOLD: clean-up can either mean +# packing files together toward the end of the file or, if file are +# deleted at the end of the file, clean up can simply mean erasing +# the end of FLASH memory so that it can be re-used again. However, +# doing this can also harm the life of the FLASH part because it can +# mean that the tail end of the FLASH is re-used too often. This +# threshold determines if/when it is worth erased the tail end of FLASH +# and making it available for re-use (and possible over-wear). +# Default: 8192. +# CONFIG_FS_ROMFS - Enable ROMFS filesystem support +# CONFIG_FS_RAMMAP - For file systems that do not support XIP, this +# option will enable a limited form of memory mapping that is +# implemented by copying whole files into memory. +# +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_FS_ROMFS=y + +# +# SPI-based MMC/SD driver +# +# CONFIG_MMCSD_NSLOTS +# Number of MMC/SD slots supported by the driver +# CONFIG_MMCSD_READONLY +# Provide read-only access (default is read/write) +# CONFIG_MMCSD_SPICLOCK - Maximum SPI clock to drive MMC/SD card. +# Default is 20MHz, current setting 24 MHz +# +CONFIG_MMCSD_NSLOTS=1 +CONFIG_MMCSD_READONLY=n +CONFIG_MMCSD_SPICLOCK=24000000 + +# +# Block driver buffering +# +# CONFIG_FS_READAHEAD +# Enable read-ahead buffering +# CONFIG_FS_WRITEBUFFER +# Enable write buffering +# +CONFIG_FS_READAHEAD=n +CONFIG_FS_WRITEBUFFER=n + +# +# RTC Configuration +# +# CONFIG_RTC - Enables general support for a hardware RTC. Specific +# architectures may require other specific settings. +# CONFIG_RTC_DATETIME - There are two general types of RTC: (1) A simple +# battery backed counter that keeps the time when power is down, and (2) +# A full date / time RTC the provides the date and time information, often +# in BCD format. If CONFIG_RTC_DATETIME is selected, it specifies this +# second kind of RTC. In this case, the RTC is used to "seed" the normal +# NuttX timer and the NuttX system timer provides for higher resoution +# time. +# CONFIG_RTC_HIRES - If CONFIG_RTC_DATETIME not selected, then the simple, +# battery backed counter is used. There are two different implementations +# of such simple counters based on the time resolution of the counter: +# The typical RTC keeps time to resolution of 1 second, usually +# supporting a 32-bit time_t value. In this case, the RTC is used to +# "seed" the normal NuttX timer and the NuttX timer provides for higher +# resoution time. If CONFIG_RTC_HIRES is enabled in the NuttX configuration, +# then the RTC provides higher resolution time and completely replaces the +# system timer for purpose of date and time. +# CONFIG_RTC_FREQUENCY - If CONFIG_RTC_HIRES is defined, then the frequency +# of the high resolution RTC must be provided. If CONFIG_RTC_HIRES is +# not defined, CONFIG_RTC_FREQUENCY is assumed to be one. +# CONFIG_RTC_ALARM - Enable if the RTC hardware supports setting of an +# alarm. A callback function will be executed when the alarm goes off +# +CONFIG_RTC=n +CONFIG_RTC_DATETIME=y +CONFIG_RTC_HIRES=n +CONFIG_RTC_FREQUENCY=n +CONFIG_RTC_ALARM=n + +# +# USB Device Configuration +# +# CONFIG_USBDEV +# Enables USB device support +# CONFIG_USBDEV_ISOCHRONOUS +# Build in extra support for isochronous endpoints +# CONFIG_USBDEV_DUALSPEED +# Hardware handles high and full speed operation (USB 2.0) +# CONFIG_USBDEV_SELFPOWERED +# Will cause USB features to indicate that the device is +# self-powered +# CONFIG_USBDEV_MAXPOWER +# Maximum power consumption in mA +# CONFIG_USBDEV_TRACE +# Enables USB tracing for debug +# CONFIG_USBDEV_TRACE_NRECORDS +# Number of trace entries to remember +# +CONFIG_USBDEV=y +CONFIG_USBDEV_ISOCHRONOUS=n +CONFIG_USBDEV_DUALSPEED=n +CONFIG_USBDEV_SELFPOWERED=y +CONFIG_USBDEV_REMOTEWAKEUP=n +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USBDEV_TRACE=n +CONFIG_USBDEV_TRACE_NRECORDS=512 + +# +# USB serial device class driver (Standard CDC ACM class) +# +# CONFIG_CDCACM +# Enable compilation of the USB serial driver +# CONFIG_CDCACM_CONSOLE +# Configures the CDC/ACM serial port as the console device. +# CONFIG_CDCACM_EP0MAXPACKET +# Endpoint 0 max packet size. Default 64 +# CONFIG_CDCACM_EPINTIN +# The logical 7-bit address of a hardware endpoint that supports +# interrupt IN operation. Default 2. +# CONFIG_CDCACM_EPINTIN_FSSIZE +# Max package size for the interrupt IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPINTIN_HSSIZE +# Max package size for the interrupt IN endpoint if high speed mode. +# Default 64 +# CONFIG_CDCACM_EPBULKOUT +# The logical 7-bit address of a hardware endpoint that supports +# bulk OUT operation. Default 4. +# CONFIG_CDCACM_EPBULKOUT_FSSIZE +# Max package size for the bulk OUT endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKOUT_HSSIZE +# Max package size for the bulk OUT endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_EPBULKIN +# The logical 7-bit address of a hardware endpoint that supports +# bulk IN operation. Default 3. +# CONFIG_CDCACM_EPBULKIN_FSSIZE +# Max package size for the bulk IN endpoint if full speed mode. +# Default 64. +# CONFIG_CDCACM_EPBULKIN_HSSIZE +# Max package size for the bulk IN endpoint if high speed mode. +# Default 512. +# CONFIG_CDCACM_NWRREQS and CONFIG_CDCACM_NRDREQS +# The number of write/read requests that can be in flight. +# Default 256. +# CONFIG_CDCACM_VENDORID and CONFIG_CDCACM_VENDORSTR +# The vendor ID code/string. Default 0x0525 and "NuttX" +# 0x0525 is the Netchip vendor and should not be used in any +# products. This default VID was selected for compatibility with +# the Linux CDC ACM default VID. +# CONFIG_CDCACM_PRODUCTID and CONFIG_CDCACM_PRODUCTSTR +# The product ID code/string. Default 0xa4a7 and "CDC/ACM Serial" +# 0xa4a7 was selected for compatibility with the Linux CDC ACM +# default PID. +# CONFIG_CDCACM_RXBUFSIZE and CONFIG_CDCACM_TXBUFSIZE +# Size of the serial receive/transmit buffers. Default 256. +# +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=n +#CONFIG_CDCACM_EP0MAXPACKET +CONFIG_CDCACM_EPINTIN=1 +#CONFIG_CDCACM_EPINTIN_FSSIZE +#CONFIG_CDCACM_EPINTIN_HSSIZE +CONFIG_CDCACM_EPBULKOUT=3 +#CONFIG_CDCACM_EPBULKOUT_FSSIZE +#CONFIG_CDCACM_EPBULKOUT_HSSIZE +CONFIG_CDCACM_EPBULKIN=2 +#CONFIG_CDCACM_EPBULKIN_FSSIZE +#CONFIG_CDCACM_EPBULKIN_HSSIZE +#CONFIG_CDCACM_NWRREQS +#CONFIG_CDCACM_NRDREQS +CONFIG_CDCACM_VENDORID=0x26AC +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6" +#CONFIG_CDCACM_RXBUFSIZE +#CONFIG_CDCACM_TXBUFSIZE + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# +# If CONFIG_NSH_TELNET is selected: +# CONFIG_NSH_IOBUFFER_SIZE -- Telnetd I/O buffer size +# CONFIG_NSH_DHCPC - Obtain address using DHCP +# CONFIG_NSH_IPADDR - Provides static IP address +# CONFIG_NSH_DRIPADDR - Provides static router IP address +# CONFIG_NSH_NETMASK - Provides static network mask +# CONFIG_NSH_NOMAC - Use a bogus MAC address +# +# If CONFIG_NSH_ROMFSETC is selected: +# CONFIG_NSH_ROMFSMOUNTPT - ROMFS mountpoint +# CONFIG_NSH_INITSCRIPT - Relative path to init script +# CONFIG_NSH_ROMFSDEVNO - ROMFS RAM device minor +# CONFIG_NSH_ROMFSSECTSIZE - ROMF sector size +# CONFIG_NSH_FATDEVNO - FAT FS RAM device minor +# CONFIG_NSH_FATSECTSIZE - FAT FS sector size +# CONFIG_NSH_FATNSECTORS - FAT FS number of sectors +# CONFIG_NSH_FATMOUNTPT - FAT FS mountpoint +# +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAX_ARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_DISABLESCRIPT=n +CONFIG_NSH_DISABLEBG=n +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_CONSOLE=y +CONFIG_NSH_USBCONSOLE=n +CONFIG_NSH_USBCONDEV="/dev/ttyACM0" +CONFIG_NSH_TELNET=n +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_IOBUFFER_SIZE=512 +CONFIG_NSH_DHCPC=n +CONFIG_NSH_NOMAC=y +CONFIG_NSH_IPADDR=0x0a000002 +CONFIG_NSH_DRIPADDR=0x0a000001 +CONFIG_NSH_NETMASK=0xffffff00 +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=1 # SIM CHANGE +CONFIG_NSH_ROMFSSECTSIZE=128 # Default 64, increased to allow for more than 64 folders on the sdcard +CONFIG_NSH_FATDEVNO=2 # SIM CHANGE +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT=/tmp + +# +# Architecture-specific NSH options +# +CONFIG_NSH_MMCSDSPIPORTNO=3 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDMINOR=0 + + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3240G-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +# Idle thread needs 4096 bytes +# default 1 KB is not enough +# 4096 bytes +CONFIG_IDLETHREAD_STACKSIZE=6000 +# USERMAIN stack size probably needs to be around 4096 bytes +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= diff --git a/nuttx/configs/px4fmu_sim/nsh/setenv.sh b/nuttx/configs/px4fmu_sim/nsh/setenv.sh new file mode 100755 index 000000000000..265520997da2 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx/configs/px4fmu_sim/src/Makefile b/nuttx/configs/px4fmu_sim/src/Makefile new file mode 100644 index 000000000000..40ea15f7ed14 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/src/Makefile @@ -0,0 +1,95 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = up_sim.c up_serial.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +HOSTSRCS = up_hosttcp.c +HOSTOBJS = $(HOSTSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) $(HOSTSRCS) +OBJS = $(AOBJS) $(COBJS) $(HOSTOBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +HOSTCFLAGS= + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +$(HOSTOBJS): %$(OBJEXT): %.c + $(Q) echo "CC: $<" + $(Q) $(CC) -c $(HOSTCFLAGS) $< -o $@ + +libboard$(LIBEXT): $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $@, $${obj}); \ + done ; ) + touch $@ + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f libboard$(LIBEXT) *~ .*.swp + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep + diff --git a/nuttx/configs/px4fmu_sim/src/up_hosttcp.c b/nuttx/configs/px4fmu_sim/src/up_hosttcp.c new file mode 100644 index 000000000000..61e2e689c243 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/src/up_hosttcp.c @@ -0,0 +1,76 @@ +/* A simple server in the internet domain using TCP + The port number is passed as an argument */ +#include +#include +#include +#include +#include +#include +#include + +int hosttcp_init(int portno); +size_t hosttcp_read(int fd, char * buffer, size_t len); +size_t hosttcp_write(int fd, const char * buffer, size_t len); +void hosttcp_close(int fd); + +int +hosttcp_init(int portno) +{ + int sockfd, newsockfd; + socklen_t clilen; + struct sockaddr_in serv_addr, cli_addr; + sockfd = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd < 0) + { + printf("error opening tcp:%d\n",portno); + return NULL; + } + + bzero((char *) &serv_addr, sizeof(serv_addr)); + serv_addr.sin_family = AF_INET; + serv_addr.sin_addr.s_addr = INADDR_ANY; + serv_addr.sin_port = htons(portno); + + printf("listening on tcp:%d\n", portno); + if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) + { + printf("error binding socket\n"); + return NULL; + } + listen(sockfd,5); + clilen = sizeof(cli_addr); + newsockfd = accept(sockfd, + (struct sockaddr *) &cli_addr, + &clilen); + close(sockfd); + if (newsockfd < 0) { + printf("error on accept"); + return NULL; + } + return newsockfd; +} + +size_t +hosttcp_read(int fd, char * buffer, size_t len) +{ + size_t n = 0; + bzero(buffer,len); + n = recv(fd,buffer,len,0); + if (n < 0) error("ERROR reading from socket"); + return n; +} + +size_t +hosttcp_write(int fd, const char * buffer, size_t len) +{ + size_t n = 0; + n = send(fd,buffer,len,0); + if (n < 0) error("ERROR writing to socket"); + return n; +} + +void +hosttcp_close(int fd) +{ + close(fd); +} diff --git a/nuttx/configs/px4fmu_sim/src/up_serial.c b/nuttx/configs/px4fmu_sim/src/up_serial.c new file mode 100644 index 000000000000..b9d8bf8d1a9b --- /dev/null +++ b/nuttx/configs/px4fmu_sim/src/up_serial.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * up_serial.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include + +/**************************************************************************** + * Definitions + ****************************************************************************/ +extern int hosttcp_init(int portno); +extern size_t hosttcp_read(int fd, char * buffer, size_t len); +extern size_t hosttcp_write(int fd, const char * buffer, size_t len); +extern void hosttcp_close(int fd); + +#ifndef CONFIG_DISABLE_POLL +#define SERIAL_PORT_TCP_SIM(DEV) \ + static int fd_##DEV = 0; \ + void up_##DEV(const int port); \ + void down_##DEV(void); \ + static ssize_t read_##DEV(struct file *, char *, size_t); \ + static ssize_t write_##DEV(struct file *, const char *, size_t); \ + static int poll_##DEV(FAR struct file *filep, FAR struct pollfd *fds, \ + bool setup); \ + static const struct file_operations fops_##DEV = \ + { \ + .read = read_##DEV, \ + .write = write_##DEV, \ + .poll = poll_##DEV, \ + }; \ + static ssize_t read_##DEV(struct file *filp, char *buffer, size_t len) \ + { \ + return hosttcp_read(fd_##DEV,buffer, len); \ + } \ + static ssize_t write_##DEV(struct file *filp, const char *buffer, size_t len) \ + { \ + return hosttcp_write(fd_##DEV,buffer, len); \ + } \ + static int poll_##DEV(FAR struct file *filep, FAR struct pollfd *fds, \ + bool setup) \ + { \ + return OK; \ + } \ + void up_##DEV(const int portno) \ + { \ + fd_##DEV = hosttcp_init(portno); \ + ASSERT(fd_##DEV != NULL); \ + register_driver("/dev/" #DEV, &fops_##DEV, 0666, NULL); \ + } \ + void down_##DEV(void) \ + { \ + hosttcp_close(fd_##DEV); \ + } +#else +#define SERIAL_PORT_TCP_SIM(DEV) \ + static int fd_##DEV = 0; \ + void up_##DEV(const int port); \ + void down_##DEV(void); \ + static ssize_t read_##DEV(struct file *, char *, size_t); \ + static ssize_t write_##DEV(struct file *, const char *, size_t); \ + static const struct file_operations fops_##DEV = \ + { \ + .read = read_##DEV, \ + .write = write_##DEV, \ + }; \ + static ssize_t read_##DEV(struct file *filp, char *buffer, size_t len) \ + { \ + return hosttcp_read(fd_##DEV,buffer, len); \ + } \ + static ssize_t write_##DEV(struct file *filp, const char *buffer, size_t len) \ + { \ + return hosttcp_write(fd_##DEV,buffer, len); \ + } \ + void \ + up_##DEV(const int portno) \ + { \ + fd_##DEV = hosttcp_init(portno); \ + ASSERT(fd_##DEV != NULL); \ + register_driver("/dev/" #DEV, &fops_##DEV, 0666, NULL); \ + } \ + void \ + down_##DEV(void) \ + { \ + hosttcp_close(fd_##DEV); \ + } +#endif + +SERIAL_PORT_TCP_SIM(ttyS0) +SERIAL_PORT_TCP_SIM(ttyS1) +SERIAL_PORT_TCP_SIM(ttyS2) diff --git a/nuttx/configs/px4fmu_sim/src/up_sim.c b/nuttx/configs/px4fmu_sim/src/up_sim.c new file mode 100644 index 000000000000..5cdc58a1b4e1 --- /dev/null +++ b/nuttx/configs/px4fmu_sim/src/up_sim.c @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4 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 + ****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* + * PPM decoder tuning parameters + */ +# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +# define PPM_MIN_START 2500 /* shortest valid start gap */ + +/**************************************************************************** +* Public Data +****************************************************************************/ + +/* decoded PPM buffer */ +#define PPM_MAX_CHANNELS 12 +uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +unsigned ppm_decoded_channels; +uint64_t ppm_last_valid_decode = 0; + +/**************************************************************************** + * Extern functions + ****************************************************************************/ +extern void up_ttyS1(int port); +extern void down_ttyS1(void); +extern void cpuload_initialize_once(void); + + +/**************************************************************************** + * Public functions + ****************************************************************************/ + +void up_systemreset(void); + +/* irq */ +void up_disable_irq(void); +void up_enable_irq(void); + +/* spi */ +void up_spiinitialize(void); + +/* ppm */ +void ppm_input_init(unsigned count_max); +void ppm_input_decode(unsigned reset, unsigned count); +int nsh_archinitialize(void); + +/* power management */ +void pm_initialize(void); +enum pm_state_e pm_checkstate(void); +int pm_changestate(enum pm_state_e newstate); + +/**************************************************************************** + * Private functions + ****************************************************************************/ +static void uninit_arch(void); + +/************************************************************************************ + * Name: up_systemreset + * + * Description: + * Reset the system. + * + ************************************************************************************/ +void +up_systemreset(void) +{ + uninit_arch(); +} + +/************************************************************************************ + * Name: up_i2cinitialize + * + * Description: + * Initialize one I2C bus + * + ************************************************************************************/ + +FAR struct i2c_dev_s *up_i2cinitialize(int port) +{ + struct i2c_dev_s * inst = NULL; + if (!(inst = kmalloc( sizeof(struct i2c_dev_s)))) + { + return NULL; + } + + struct i2c_ops_s * ops = NULL; + if (!(ops = kmalloc( sizeof(struct i2c_ops_s)))) + { + return NULL; + } + + /* Initialize instance */ + inst->ops = &ops; + return inst; +} + +/************************************************************************************ + * Name: up_i2cuninitialize + * + * Description: + * Uninitialize an I2C bus + * + ************************************************************************************/ + +int up_i2cuninitialize(FAR struct i2c_dev_s * dev) +{ + ASSERT(dev); + kfree(dev); + return OK; +} + +/************************************************************************************ + * Name: up_i2creset + * + * Description: + * Reset an I2C bus + * + ************************************************************************************/ +int up_i2creset(FAR struct i2c_dev_s * dev) +{ + ASSERT(dev); + return OK; +} + + +void +up_disable_irq(void) +{ +} + +void +up_enable_irq(void) +{ +} + +void +up_spiinitialize(void) +{ +} + +void +ppm_input_init(unsigned count_max) +{ +} + +void +ppm_input_decode(unsigned reset, unsigned count) +{ +} + +int +nsh_archinitialize(void) +{ + vdbg("initializing sim arch\n"); + up_ttyS1(5501); + atexit(uninit_arch); + return 0; +} + +void +pm_initialize(void) +{ + cpuload_initialize_once(); +} + +enum pm_state_e +pm_checkstate(void) +{ + return 0; +} + +int +pm_changestate(enum pm_state_e newstate) +{ + return 0; +} + +void +uninit_arch(void) +{ + vdbg("uninit"); + down_ttyS1(); + ASSERT(0); +} diff --git a/nuttx/configs/px4io/Kconfig b/nuttx/configs/px4io/Kconfig new file mode 100644 index 000000000000..b5e6d5515f97 --- /dev/null +++ b/nuttx/configs/px4io/Kconfig @@ -0,0 +1,26 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_BOARD_PX4FMU +config ARCH_LEDS + bool "NuttX LED support" + default n + ---help--- + "Support control of board LEDs by NuttX to indicate system state" + +config ARCH_BUTTONS + bool "Button support" + default n + ---help--- + "Support interfaces to use buttons provided by the board." + +config ARCH_IRQBUTTONS + bool "Button interrupt support" + default n + depends on ARCH_BUTTONS + ---help--- + "Support EXTI interrupts on button presses and releases." + +endif diff --git a/nuttx/sched/task_exithook.c b/nuttx/sched/task_exithook.c index e94476f2a73a..004d09e200cb 100644 --- a/nuttx/sched/task_exithook.c +++ b/nuttx/sched/task_exithook.c @@ -126,6 +126,7 @@ static inline void task_atexit(FAR _TCB *tcb) tcb->atexitfunc = NULL; } #endif +} #else # define task_atexit(tcb) #endif