diff --git a/.github/workflows/cflite_batch.yml b/.github/workflows/cflite_batch.yml deleted file mode 100644 index d1321cc6b7fb..000000000000 --- a/.github/workflows/cflite_batch.yml +++ /dev/null @@ -1,34 +0,0 @@ -name: ClusterFuzzLite batch fuzzing -on: - schedule: - - cron: '0 6 * * *' # UTC 6am every day. -permissions: read-all -jobs: - BatchFuzzing: - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - sanitizer: - - address - - undefined - - memory - steps: - - name: Build Fuzzers (${{ matrix.sanitizer }}) - id: build - uses: google/clusterfuzzlite/actions/build_fuzzers@v1 - with: - sanitizer: ${{ matrix.sanitizer }} - - name: Run Fuzzers (${{ matrix.sanitizer }}) - id: run - uses: google/clusterfuzzlite/actions/run_fuzzers@v1 - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - fuzz-seconds: 1800 # 30 mins - mode: 'batch' - sanitizer: ${{ matrix.sanitizer }} - # Optional but recommended: For storing certain artifacts from fuzzing. - # See later section on "Git repo for storage". - # storage-repo: https://${{ secrets.PERSONAL_ACCESS_TOKEN }}@github.com/OWNER/STORAGE-REPO-NAME.git - # storage-repo-branch: main # Optional. Defaults to "main" - # storage-repo-branch-coverage: gh-pages # Optional. Defaults to "gh-pages". diff --git a/.github/workflows/tiiuae-builder-images.yaml b/.github/workflows/tiiuae-builder-images.yaml new file mode 100644 index 000000000000..153ddb71f1bf --- /dev/null +++ b/.github/workflows/tiiuae-builder-images.yaml @@ -0,0 +1,38 @@ +name: tiiuae-builder-images + +on: + # Run only manually + workflow_dispatch: + +jobs: + hw-builder-image: + name: create docker builder base image + runs-on: ubuntu-latest + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v3 + with: + path: px4-firmware + fetch-depth: 0 + - name: Docker meta + id: meta + uses: docker/metadata-action@v3 + with: + images: ghcr.io/tiiuae/px4-firmware-builder-base + tags: | + type=raw,value=latest + type=sha + - name: Login to GitHub Container Registry + uses: docker/login-action@v2 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Build builder base image and push + uses: docker/build-push-action@v3 + with: + context: . + file: ./px4-firmware/packaging/Dockerfile.build_env_pre + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} diff --git a/.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml b/.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml new file mode 100644 index 000000000000..2cc43df443fc --- /dev/null +++ b/.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml @@ -0,0 +1,41 @@ +on: + workflow_call: + inputs: + product: + required: true + type: string + enabled: + required: false + type: boolean + default: true + secrets: + GH_REPO_TOKEN: + required: false +jobs: + build: + runs-on: ubuntu-latest + if: ${{ inputs.enabled }} + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v3 + with: + token: ${{ secrets.GH_REPO_TOKEN }} + # if we clone_public, then do not clone submodules, otherwise 'recursive' + submodules: ${{ inputs.clone_public == true && 'false' || 'recursive' }} + path: px4-firmware + fetch-depth: 0 + - name: Run px4-firmware ${{ inputs.product }} build + run: | + set -eux + mkdir -p bin + cd px4-firmware/ + # run clone_public.sh if clone_public flag is provided + ./clone_public.sh + ./build.sh ../bin/ ${{ inputs.product }} + ls ../bin + - name: Upload ${{ inputs.product }} + uses: actions/upload-artifact@v3 + with: + name: pixhawk + path: bin/ + retention-days: 1 diff --git a/.github/workflows/tiiuae-pixhawk-and-saluki.yaml b/.github/workflows/tiiuae-pixhawk-and-saluki.yaml new file mode 100644 index 000000000000..ff4d7a28910b --- /dev/null +++ b/.github/workflows/tiiuae-pixhawk-and-saluki.yaml @@ -0,0 +1,281 @@ +name: tiiuae-pixhawk-and-saluki + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +jobs: + fc_matrix: + strategy: + matrix: + product: [pixhawk, saluki-v1_default, saluki-v1_bootloader, saluki-v1_amp, saluki-v1_protected, saluki-v2_default, saluki-v2_bootloader, saluki-v2_amp, saluki-v2_protected] + uses: ./.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml + with: + product: ${{ matrix.product }} + # old workflow had condition to run only if PR is done to current repo (or triggered with other event) + enabled: ${{ github.event.pull_request.head.repo.full_name == github.repository || github.event_name == 'push' || github.event_name == 'workflow_dispatch' }} + secrets: inherit + + px4fwupdater: + name: build px4fwupdater + runs-on: ubuntu-latest + needs: + - fc_matrix + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v3 + with: + path: px4-firmware + fetch-depth: 0 + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - name: Run px4-firmware px4fwupdater build + run: | + set -eux + mkdir -p bin + cd px4-firmware/ + ./clone_public.sh + ./build.sh ../bin/ px4fwupdater + ls ../bin + - name: Upload px4fwupdater to tmp storage + uses: actions/upload-artifact@v3 + with: + name: pixhawk + path: bin/ + retention-days: 1 + + upload-px4fwupdater: + name: upload px4fwupdater to docker registry + runs-on: ubuntu-latest + needs: + - px4fwupdater + if: true + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v3 + with: + path: px4-firmware + fetch-depth: 0 + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - name: Firmware flasher - Container metadata + id: containermeta # referenced from later step + uses: docker/metadata-action@v4 + with: + images: ghcr.io/tiiuae/px4-firmware + tags: | + type=ref,event=branch + type=ref,event=pr + type=semver,pattern={{version}} + type=sha + - name: Login to GitHub Container Registry + uses: docker/login-action@v2 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Firmware flasher - Build and push + uses: docker/build-push-action@v3 + with: + push: true + context: . + file: px4-firmware/Tools/px_uploader.Dockerfile + tags: ${{ steps.containermeta.outputs.tags }} + labels: ${{ steps.containermeta.outputs.labels }} + + upload-px4fwupdater-uae: + name: upload px4fwupdater to UAE docker registry + runs-on: ubuntu-latest + needs: + - px4fwupdater + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v3 + with: + path: px4-firmware + fetch-depth: 0 + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - name: Firmware flasher - Container metadata + id: containermeta # referenced from later step + uses: docker/metadata-action@v4 + with: + images: artifactory.ssrcdevops.tii.ae/tiiuae/px4-firmware + tags: | + type=ref,event=branch + type=ref,event=pr + type=semver,pattern={{version}} + type=sha + - name: Login to SSRC JFrog Container Registry + uses: docker/login-action@v2 + with: + registry: artifactory.ssrcdevops.tii.ae + username: ${{ secrets.UAE_RT_USER }} + password: ${{ secrets.UAE_RT_APIKEY }} + - name: Firmware flasher - Build and push + uses: docker/build-push-action@v3 + with: + push: true + context: . + file: px4-firmware/Tools/px_uploader.Dockerfile + tags: ${{ steps.containermeta.outputs.tags }} + labels: ${{ steps.containermeta.outputs.labels }} + + artifactory: + name: upload builds to artifactory + runs-on: ubuntu-latest + needs: + - px4fwupdater + - fc_matrix + if: true + steps: + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - uses: jfrog/setup-jfrog-cli@v3 + env: + JF_ENV_1: ${{ secrets.ARTIFACTORY_CLOUD_TOKEN }} + - name: Upload px4-firmware build to Artifactory + env: + ARTIFACTORY_GEN_REPO: ssrc-gen-public-local + BUILD_NAME_PX4: px4-firmware + CI: true + run: | + set -exu + + pr_or_empty="" + if [ ${{ github.event_name }} == 'pull_request' ]; then + pr_or_empty="pr/" + fi + + for pkg in $(find bin -type f); do + + file_name=$(basename $pkg) + ext="${file_name##*.}" + target_path="" + pkg_name=$(echo $file_name | sed -r -e 's/-[0-9]+\.[0-9]+\.[0-9]+-.*//g') + + if [[ $file_name = px4_fmu* ]]; then + target_path="pixhawk" + elif [[ $file_name = ssrc_saluki* ]]; then + target_path="saluki" + else + echo "$pkg ignored" + continue + fi + + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME_PX4" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty}$file_name" + + jfrog rt cp --flat \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty}$file_name" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty}latest/${pkg_name}.${ext}" + done + + jfrog rt build-publish "$BUILD_NAME_PX4" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME_PX4" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + + + artifactory-uae: + name: upload builds to UAE artifactory + runs-on: ubuntu-latest + needs: + - px4fwupdater + - fc_matrix + if: true + steps: + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - uses: jfrog/setup-jfrog-cli@v3 + env: + JF_ENV_1: ${{ secrets.UAE_ARTIFACTORY_TOKEN }} + - name: Upload px4-firmware build to Artifactory + env: + ARTIFACTORY_GEN_REPO: gen-public-local + BUILD_NAME_PX4: px4-firmware + CI: true + run: | + set -exu + + pr_or_empty="" + if [ ${{ github.event_name }} == 'pull_request' ]; then + pr_or_empty="pr/" + fi + + for pkg in $(find bin -type f); do + + file_name=$(basename $pkg) + ext="${file_name##*.}" + target_path="" + pkg_name=$(echo $file_name | sed -r -e 's/-[0-9]+\.[0-9]+\.[0-9]+-.*//g') + + if [[ $file_name = px4_fmu* ]]; then + target_path="pixhawk" + elif [[ $file_name = ssrc_saluki* ]]; then + target_path="saluki" + else + echo "$pkg ignored" + continue + fi + + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME_PX4" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty}$file_name" + + jfrog rt cp --flat \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty}$file_name" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty}latest/${pkg_name}.${ext}" + done + + jfrog rt build-publish "$BUILD_NAME_PX4" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME_PX4" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + + - name: Upload px4-fwupdater build to Artifactory + env: + ARTIFACTORY_DEB_REPO: debian-public-local + DISTRIBUTION: focal + COMPONENT: fog-sw + ARCHITECTURE: amd64 + BUILD_NAME_DEB: px4-fwupdater + CI: true + run: | + set -exu + pkg=$(find bin -name 'px4fwupdater*.deb') + pkg_name=$(basename $pkg) + jfrog rt u --deb "$DISTRIBUTION/$COMPONENT/$ARCHITECTURE" \ + --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME_DEB" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_DEB_REPO/$pkg_name" + jfrog rt build-publish "$BUILD_NAME_DEB" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME_DEB" "$GITHUB_SHA" "$ARTIFACTORY_DEB_REPO" \ + --status dev \ + --comment "development build" diff --git a/.github/workflows/tiiuae-sitl-tests.yml b/.github/workflows/tiiuae-sitl-tests.yml new file mode 100644 index 000000000000..366dacae758b --- /dev/null +++ b/.github/workflows/tiiuae-sitl-tests.yml @@ -0,0 +1,68 @@ +name: SITL Tests + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + config: + - {model: "iris", latitude: "59.617693", longitude: "-151.145316", altitude: "48", build_type: "RelWithDebInfo" } # Alaska + container: + image: ghcr.io/tiiuae/px4-firmware-builder-base:latest + credentials: + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + steps: + - uses: actions/checkout@v3 + with: + token: ${{ secrets.GH_REPO_TOKEN }} + fetch-depth: 0 + - name: Install dependencies + run: | + apt update -y && apt install -y --no-install-recommends wget + pip3 install requests + git config --global --add safe.directory '*' + - name: Download MAVSDK + run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" + - name: Install MAVSDK + run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" + - name: check environment + env: + PX4_HOME_LAT: ${{matrix.config.latitude}} + PX4_HOME_LON: ${{matrix.config.longitude}} + PX4_HOME_ALT: ${{matrix.config.altitude}} + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: | + export + - name: Build PX4 + env: + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: make px4_sitl_default + - name: Build SITL Gazebo + env: + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: | + . /opt/ros/humble/setup.sh + make px4_sitl_default sitl_gazebo + - name: Build MAVSDK tests + env: + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + DONT_RUN: 1 + run: make px4_sitl_default gazebo mavsdk_tests + - name: Run SITL tests + env: + PX4_HOME_LAT: ${{matrix.config.latitude}} + PX4_HOME_LON: ${{matrix.config.longitude}} + PX4_HOME_ALT: ${{matrix.config.altitude}} + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose + timeout-minutes: 45 diff --git a/.github/workflows/tiiuae-sitl.yaml b/.github/workflows/tiiuae-sitl.yaml new file mode 100644 index 000000000000..d1bebfd2233f --- /dev/null +++ b/.github/workflows/tiiuae-sitl.yaml @@ -0,0 +1,121 @@ +name: tii-px4-sitl + +on: + push: + branches: [ main ] + pull_request: + +jobs: + tii-px4-sitl: + runs-on: ubuntu-latest + services: + registry: + image: registry:2 + ports: + - 5000:5000 + steps: + + - name: Checkout px4-firmware + uses: actions/checkout@v3 + with: + path: px4-firmware + fetch-depth: 0 + + - uses: docker/setup-buildx-action@v2 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + with: + driver-opts: network=host + + # Run docker build + - name: Run fog-sw docker build + run: | + set -eux + mkdir bin + cd px4-firmware + ./clone_public.sh + ./build_sitl.sh ../bin/ + ls ../bin/ + + - name: Docker meta + id: meta + uses: docker/metadata-action@v4 + with: + images: ghcr.io/tiiuae/tii-px4-sitl + tags: | + type=ref,event=branch + type=ref,event=pr + type=semver,pattern={{version}} + type=sha + type=raw,value=latest + + - name: Login to GitHub Container Registry + uses: docker/login-action@v2 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Build tii-px4-sitl image and push + uses: docker/build-push-action@v3 + with: + context: . + file: ./px4-firmware/packaging/Dockerfile.sitl + pull: true + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + + - uses: jfrog/setup-jfrog-cli@v3 + if: github.event_name == 'push' + env: + JF_ENV_1: ${{ secrets.ARTIFACTORY_CLOUD_TOKEN }} + + - name: Upload to Artifactory (px4-sitl) + env: + ARTIFACTORY_GEN_REPO: ssrc-gen-public-local + BUILD_NAME: px4-sitl + CI: true + if: github.event_name == 'push' + run: | + set -exu + jfrog rt ping + pkg=$(find bin -name 'px4_sitl_build*.tar.gz') + pkg_name=$(basename $pkg) + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/sitl/$pkg_name" + jfrog rt build-publish "$BUILD_NAME" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + jfrog rt cp --flat \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/sitl/$pkg_name" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/sitl/latest/px4_sitl_build.tar.gz" + + - name: Upload to Artifactory (px4-gazebo-data) + env: + ARTIFACTORY_GEN_REPO: ssrc-gen-public-local + BUILD_NAME: gazebo-data + CI: true + if: github.event_name == 'push' + run: | + set -exu + jfrog rt ping + pkg=$(find bin -name 'px4_gazebo_data*.tar.gz') + pkg_name=$(basename $pkg) + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_GEN_REPO/builds/gazebo-data/$pkg_name" + jfrog rt build-publish "$BUILD_NAME" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + jfrog rt cp --flat \ + "$ARTIFACTORY_GEN_REPO/builds/gazebo-data/$pkg_name" \ + "$ARTIFACTORY_GEN_REPO/builds/gazebo-data/latest/px4_gazebo_data.tar.gz" diff --git a/.gitmodules b/.gitmodules index a8e90fddc241..952b2238e176 100644 --- a/.gitmodules +++ b/.gitmodules @@ -12,7 +12,7 @@ branch = main [submodule "Tools/simulation/gazebo/sitl_gazebo"] path = Tools/simulation/gazebo/sitl_gazebo - url = https://github.com/PX4/PX4-SITL_gazebo.git + url = https://github.com/tiiuae/PX4-SITL_gazebo.git branch = main [submodule "src/drivers/gps/devices"] path = src/drivers/gps/devices @@ -20,8 +20,8 @@ branch = main [submodule "platforms/nuttx/NuttX/nuttx"] path = platforms/nuttx/NuttX/nuttx - url = https://github.com/PX4/NuttX.git - branch = px4_firmware_nuttx-10.3.0+ + url = https://github.com/tiiuae/nuttx.git + branch = master [submodule "platforms/nuttx/NuttX/apps"] path = platforms/nuttx/NuttX/apps url = https://github.com/PX4/NuttX-apps.git @@ -60,5 +60,19 @@ branch = px4 [submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"] path = src/modules/microdds_client/Micro-XRCE-DDS-Client - url = https://github.com/PX4/Micro-XRCE-DDS-Client.git - branch = px4 + url = https://github.com/tiiuae/Micro-XRCE-DDS-Client.git +[submodule "boards/ssrc/saluki-v1"] + path = boards/ssrc/saluki-v1 + url = ../saluki-v1.git +[submodule "boards/ssrc/saluki-v2"] + path = boards/ssrc/saluki-v2 + url = ../saluki-v2.git +[submodule "src/drivers/pfsoc_crypto"] + path = src/drivers/pfsoc_crypto + url = ../pfsoc_crypto.git +[submodule "src/drivers/pfsoc_keystore"] + path = src/drivers/pfsoc_keystore + url = ../pfsoc_keystore.git +[submodule "platforms/nuttx/NuttX/extern/pf_crypto"] + path = platforms/nuttx/NuttX/extern/pf_crypto + url = ../pf_crypto.git diff --git a/ROMFS/CMakeLists.txt b/ROMFS/CMakeLists.txt index 7ec4b96a043f..f8d1392d3bbb 100644 --- a/ROMFS/CMakeLists.txt +++ b/ROMFS/CMakeLists.txt @@ -172,6 +172,7 @@ endif() # board custom init files set(OPTIONAL_BOARD_RC) list(APPEND OPTIONAL_BOARD_RC + rc.board_paths rc.board_defaults rc.board_sensors rc.board_extras diff --git a/ROMFS/minimal_romfs/CMakeLists.txt b/ROMFS/minimal_romfs/CMakeLists.txt new file mode 100644 index 000000000000..054b2e22aa2d --- /dev/null +++ b/ROMFS/minimal_romfs/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +add_subdirectory(init.d) diff --git a/ROMFS/minimal_romfs/init.d/CMakeLists.txt b/ROMFS/minimal_romfs/init.d/CMakeLists.txt new file mode 100644 index 000000000000..194ecb5a7c6c --- /dev/null +++ b/ROMFS/minimal_romfs/init.d/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_romfs_files( + rcS +) diff --git a/ROMFS/minimal_romfs/init.d/rcS b/ROMFS/minimal_romfs/init.d/rcS new file mode 100644 index 000000000000..2ec94cf9d531 --- /dev/null +++ b/ROMFS/minimal_romfs/init.d/rcS @@ -0,0 +1,26 @@ +#!/bin/sh +# Un comment and use set +e to ignore and set -e to enable 'exit on error control' +set +e +# Un comment the line below to help debug scripts by printing a trace of the script commands +#set -x +# PX4FMU startup script. +# +# NOTE: environment variable references: +# If the dollar sign ('$') is followed by a left bracket ('{') then the +# variable name is terminated with the right bracket character ('}'). +# Otherwise, the variable name goes to the end of the argument. +# +# +# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. +# +#------------------------------------------------------------------------------ + +# +# Mount the procfs. +# +mount -t procfs /proc + +# +# Print full system version. +# +ver all diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 1ba7ccde27e6..044fabde7a05 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -38,5 +38,6 @@ px4_add_romfs_files( px4-rc.params px4-rc.simulator rc.replay + rcS.sim_tcp_server rcS ) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x b/ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x new file mode 100644 index 000000000000..73ed0882fe41 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x @@ -0,0 +1,57 @@ +#!/bin/sh +# +# @name SSRC Quad X on holybro x500 frame +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Jukka Laitinen +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.mc_defaults + +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 30 +param set MC_ROLLRATE_P 0.14 +param set MC_PITCHRATE_P 0.14 +param set MC_ROLLRATE_I 0.3 +param set MC_PITCHRATE_I 0.3 +param set MC_ROLLRATE_D 0.004 +param set MC_PITCHRATE_D 0.004 + +param set BAT_N_CELLS 4 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + +param set MAV_0_CONFIG 0 +param set RTPS_MAV_CONFIG 101 +param set SER_TEL1_BAUD 3000000 + +# Disable Multi-EKF +param set EKF2_MULTI_IMU 0 +param set SENS_IMU_MODE 1 +param set EKF2_MULTI_MAG 0 +param set SENS_MAG_MODE 1 + +# Logger used only while flying +param set SDLOG_MODE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index fadd7cfb2cc5..4d772a360c22 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -75,6 +75,7 @@ px4_add_romfs_files( 3011_hexarotor_x 4001_x500 + 4400_ssrc_fog_x 17001_tf-g1 17002_tf-g2 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params index d05f50146a42..6402f8c39fb9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params @@ -3,3 +3,23 @@ #param set-default MAV_SYS_ID $((px4_instance+1)) #param set-default IMU_INTEG_RATE 250 + +if [ -f "/enclave/fog_env" ]; +then + # read and set SSRC_CONFIG env variable + . /enclave/fog_env +fi + +if [ -d "/ssrc_config" ] && [ -z "$SSRCFILE" ]; +then + SSRCFILE="/ssrc_config/config_${SSRC_CONFIG}.txt" + # Use environment variable SSRC_CONFIG to choose config.txt file. + if [ -f "$SSRCFILE" ]; then + echo "SSRC_CONFIG: load $SSRCFILE" + . "$SSRCFILE" + else + echo "config file '$SSRCFILE' not found." + fi +else + echo "No SSRC config given." +fi diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 1d982860f2ff..e0b677c1de63 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -118,7 +118,14 @@ else if [ -z "${PX4_SIM_HOST_ADDR}" ]; then echo "PX4 SIM HOST: localhost" - simulator_mavlink start -c $simulator_tcp_port + if [ -z "${PX4_SIM_USE_TCP_SERVER}" ]; then + # Use default UDP protocol for mavlink msg transport with simulator + simulator_mavlink start -u $simulator_udp_port + else + # Use TCP server for mavlink msg transport with simulator + echo "PX4 SIM USE TCP SERVER" + simulator_mavlink start -s $simulator_tcp_port + fi else echo "PX4 SIM HOST: ${PX4_SIM_HOST_ADDR}" simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}" diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 1bcdf974fe4f..38655327ceb8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -130,6 +130,20 @@ fi # shellcheck disable=SC2154 param set MAV_SYS_ID $((px4_instance+1)) +simulator_tcp_port=$((4560+px4_instance)) +udp_offboard_port_local=$((14580+px4_instance)) +udp_offboard_port_remote=$((14540+px4_instance)) +[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps +udp_onboard_payload_port_local=$((14280+px4_instance)) +udp_onboard_payload_port_remote=$((14030+px4_instance)) +udp_onboard_gimbal_port_local=$((13030+px4_instance)) +udp_onboard_gimbal_port_remote=$((13280+px4_instance)) +udp_gcs_port_local=$((18570+px4_instance)) +udp_gcs_port_remote=14550 + +udp_gcs_addr_remote=127.0.0.1 +[ -n "$PX4_QGC_REMOTE_ADDRESS" ] && udp_gcs_addr_remote=$PX4_QGC_REMOTE_ADDRESS + if [ $AUTOCNF = yes ] then param set SYS_AUTOSTART $SYS_AUTOSTART @@ -272,7 +286,7 @@ then # Override namespace if environment variable is defined microdds_ns="-n $PX4_MICRODDS_NS" fi -microdds_client start -t udp -p 8888 $microdds_ns +microdds_client start -t udp -r 2019 -p 2020 $microdds_ns if param greater -s MNT_MODE_IN -1 then @@ -310,9 +324,9 @@ fi # Run script to start logging if param compare SYS_MC_EST_GROUP 2 then - set LOGGER_ARGS "-p ekf2_timestamps" + set LOGGER_ARGS "-p ekf2_timestamps -m file" else - set LOGGER_ARGS "-p vehicle_attitude" + set LOGGER_ARGS "-p vehicle_attitude -m file" fi . ${R}etc/init.d/rc.logging diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server b/ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server new file mode 100644 index 000000000000..b66f0b9610b4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server @@ -0,0 +1,10 @@ +#!/bin/sh + +# search path for sourcing rcS +PATH="$PATH:${R}etc/init.d-posix" + +# Define TCP server +export PX4_SIM_USE_TCP_SERVER=1 + +# Continue to the main startup script +. rcS diff --git a/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x new file mode 100644 index 000000000000..c2d448d4a278 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x @@ -0,0 +1,111 @@ +#!/bin/sh +# +# @name SSRC Quad X on holybro x500 frame +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Jukka Laitinen +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. /etc/init.d/rc.mc_defaults + +# Default rates +param set-default IMU_GYRO_CUTOFF 30 +param set-default IMU_GYRO_NF0_FRQ 75 +param set-default IMU_DGYRO_CUTOFF 30 +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.3 +param set-default MC_PITCHRATE_I 0.3 +param set-default MC_ROLLRATE_D 0.004 +param set-default MC_PITCHRATE_D 0.004 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + +# PWM functions +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + +# Set minimum PWM control values +param set-default PWM_MAIN_MIN1 1050 +param set-default PWM_MAIN_MIN2 1050 +param set-default PWM_MAIN_MIN3 1050 +param set-default PWM_MAIN_MIN4 1050 + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 + +# Increase velocity controller P gain +param set-default MPC_XY_VEL_P_ACC 2.4 + +# Battery parameters +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_EMPTY 3.6 +param set-default BAT1_V_DIV 18.1 +param set-default BAT1_R_INTERNAL -1.0 +param set-default BAT1_V_LOAD_DROP 0.3000 +param set-default BAT1_V_EMPTY 3.5000 + +param set-default BAT2_N_CELLS 4 +param set-default BAT2_V_CHARGED 4.2 +param set-default BAT2_V_EMPTY 3.6 +param set-default BAT2_V_DIV 18.1 +param set-default BAT2_R_INTERNAL -1.0 +param set-default BAT2_V_LOAD_DROP 0.3000 +param set-default BAT2_V_EMPTY 3.5000 + +# Enable LL40LS in i2c +param set-default SENS_EN_LL40LS 2 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Set default for disarm after land to 4s +param set-default COM_DISARM_LAND 4.0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Set takeoff ramp to disabled for a more decisive takeoff action +param set-default MPC_TKO_RAMP_T 0 + +# Set default logfile encryption key indecies +param set-default SDLOG_EXCH_KEY 2 +param set-default SDLOG_KEY 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor b/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor new file mode 100644 index 000000000000..9b7fcefb09cd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor @@ -0,0 +1,89 @@ +#!/bin/sh +# +# @name SSRC Quad X on T-Motor M690 series +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Jukka Laitinen +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. /etc/init.d/rc.mc_defaults + +# Default rates +param set-default IMU_GYRO_CUTOFF 60 +param set-default IMU_DGYRO_CUTOFF 30 +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.3 +param set-default MC_PITCHRATE_I 0.3 +param set-default MC_ROLLRATE_D 0.004 +param set-default MC_PITCHRATE_D 0.004 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.25 +param set-default CA_ROTOR0_PY 0.25 +param set-default CA_ROTOR1_PX -0.25 +param set-default CA_ROTOR1_PY -0.25 +param set-default CA_ROTOR2_PX 0.25 +param set-default CA_ROTOR2_PY -0.25 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.25 +param set-default CA_ROTOR3_PY 0.25 +param set-default CA_ROTOR3_KM -0.05 + +# PWM functions +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 + +# Increase velocity controller P gain +param set-default MPC_XY_VEL_P_ACC 2.4 + +# Battery parameters +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_EMPTY 3.6 +param set-default BAT1_V_DIV 18.1 + +# Enable LL40LS in i2c +param set-default SENS_EN_LL40LS 2 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Set default for disarm after land to 4s +param set-default COM_DISARM_LAND 4.0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Change current sense shunt resistor value +param set-default INA226_SHUNT 0.00025 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 8e9931aa89f8..36cea75fa5f3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -66,6 +66,8 @@ px4_add_romfs_files( 4061_atl_mantis_edu 4071_ifo 4073_ifo-s + 4400_ssrc_fog_x + 4401_ssrc_fog_x_tmotor 4500_clover4 4900_crazyflie 4901_crazyflie21 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e6012a2c23aa..a19af93c0f48 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -36,6 +36,26 @@ set SDCARD_FORMAT no set STARTUP_TUNE 1 set VEHICLE_TYPE none +if [ -d "/dev/rptun" ] +then + set LOCAL_IPADDR "192.168.201.101" + set REMOTE_IPADDR "192.168.201.240" +else + set LOCAL_IPADDR "192.168.200.101" + set REMOTE_IPADDR "192.168.200.100" +fi + +# +# Optional board path defaults: rc.board_paths +# +set BOARD_RC_PATHS ${R}etc/init.d/rc.board_paths +if [ -f $BOARD_RC_PATHS ] +then + echo "Board path defaults: ${BOARD_RC_PATHS}" + . $BOARD_RC_PATHS +fi +unset BOARD_RC_PATHS + # # Print full system version. # @@ -532,6 +552,25 @@ else # fi +# +# Start RPTUN +# This is intentionally the last process to start as this may wait a while. +# + +if [ -d "/dev/rptun" ] +then + echo "Starting RPTUN at ${LOCAL_IPADDR}" + rptun start /dev/rptun/mpfs-ihc + ifconfig rpnet ${LOCAL_IPADDR} + ifup rpnet + + # Start uXRCE-DDS client on UDP after rpnet is up & running + echo "Starting microdds_client connecting to ${REMOTE_IPADDR}" + microdds_client start -t udp -h ${REMOTE_IPADDR} -r 2019 -p 2020 +else + echo "No RPTUN device available" +fi + # # Unset all script parameters to free RAM. # @@ -550,6 +589,8 @@ unset SDCARD_EXT_PATH unset SDCARD_FORMAT unset STARTUP_TUNE unset VEHICLE_TYPE +unset LOCAL_IPADDR +unset REMOTE_IPADDR # # Boot is complete, inform MAVLink app(s) that the system is now fully up and running. diff --git a/Tools/cryptotools.py b/Tools/cryptotools.py index 6e4bd42d63e5..73b90c0ab35e 100755 --- a/Tools/cryptotools.py +++ b/Tools/cryptotools.py @@ -1,8 +1,7 @@ #!/usr/bin/env python3 -import nacl.encoding -import nacl.signing -import nacl.hash +from cryptography.hazmat.primitives.asymmetric.ed25519 import Ed25519PrivateKey +from cryptography.hazmat.primitives import serialization import struct import binascii import json @@ -11,65 +10,25 @@ from pathlib import Path import sys -def make_public_key_h_file(signing_key,key_name): - """ - This file generate the public key header file - to be included into the bootloader build. - """ - public_key_c='\n' - for i,c in enumerate(signing_key.verify_key.encode(encoder=nacl.encoding.RawEncoder)): - public_key_c+= hex(c) - public_key_c+= ', ' - if((i+1)%8==0): - public_key_c+= '\n' - with open(key_name+'.pub' ,mode='w') as f: - f.write("//Public key to verify signed binaries") - f.write(public_key_c) - -def make_key_file(signing_key, key_name): - """ - Writes the key.json file. - Attention do not override your existing key files. - Do not publish your private key!! - """ - - key_file = Path(key_name+'.json') - if key_file.is_file(): - print("ATTENTION: key.json already exists, are you sure you want to overwrite it?") - print("Remove file and run script again.") - print("Script aborted!") - sys.exit(1) - - keys={} - keys["date"] = time.asctime() - keys["public"] = (signing_key.verify_key.encode(encoder=nacl.encoding.HexEncoder)).decode() - keys["private"] = binascii.hexlify(signing_key._seed).decode() - #print (keys) - with open(key_name+'.json', "w") as write_file: - json.dump(keys, write_file) - return keys - def ed25519_sign(private_key, signee_bin): """ This function creates the signature. It takes the private key and the binary file and returns the tuple (signature, public key) """ - signing_key = nacl.signing.SigningKey(private_key, encoder=nacl.encoding.HexEncoder) - # Sign a message with the signing key - signed = signing_key.sign(signee_bin,encoder=nacl.encoding.RawEncoder) + signature = private_key.sign(signee_bin) # Obtain the verify key for a given signing key - verify_key = signing_key.verify_key - - # Serialize the verify key to send it to a third party - verify_key_hex = verify_key.encode(encoder=nacl.encoding.HexEncoder) + public_key = private_key.public_key() + verify_key = public_key.public_bytes( + encoding=serialization.Encoding.Raw, + format=serialization.PublicFormat.Raw + ) - return signed.signature, verify_key_hex + return signature, verify_key - -def sign(bin_file_path, key_file_path=None, generated_key_file=None): +def sign(bin_file_path, key_file_path=None): """ reads the binary file and the key file. If the key file does not exist, it generates a @@ -85,14 +44,14 @@ def sign(bin_file_path, key_file_path=None, generated_key_file=None): signee_bin += bytearray(b'\xff')*(4-len(signee_bin)%4) try: - with open(key_file_path,mode='r') as f: - keys = json.load(f) - #print(keys) + with open(key_file_path,mode='rb') as f: + private_key = serialization.load_pem_private_key(f.read(), None) + except: print('ERROR: Key file',key_file_path,'not found') sys.exit(1) - signature, public_key = ed25519_sign(keys["private"], signee_bin) + signature, public_key = ed25519_sign(private_key, signee_bin) # Do a sanity check. This type of signature is always 64 bytes long assert len(signature) == 64 @@ -104,28 +63,6 @@ def sign(bin_file_path, key_file_path=None, generated_key_file=None): return signee_bin + signature, public_key -def generate_key(key_file): - """ - Generate two files: - "key_file.pub" containing the public key in C-format to be included in the bootloader build - "key_file.json, containt both private and public key. - Do not leak or loose the key file. This is mandatory for signing - all future binaries you want to deploy! - """ - - # Generate a new random signing key - signing_key = nacl.signing.SigningKey.generate() - # Serialize the verify key to send it to a third party - verify_key_hex = signing_key.verify_key.encode(encoder=nacl.encoding.HexEncoder) - print("public key :",verify_key_hex) - - private_key_hex=binascii.hexlify(signing_key._seed) - print("private key :",private_key_hex) - - keys = make_key_file(signing_key,key_file) - make_public_key_h_file(signing_key,key_file) - return keys - if(__name__ == "__main__"): parser = argparse.ArgumentParser(description="""CLI tool to calculate and add signature to px4. bin files\n @@ -134,18 +71,10 @@ def generate_key(key_file): parser.add_argument("signee", help=".bin file to add signature", nargs='?', default=None) parser.add_argument("signed", help="signed output .bin", nargs='?', default=None) - parser.add_argument("--key", help="key.json file", default="Tools/test_keys/test_keys.json") + parser.add_argument("--key", help="key.json file", default="Tools/test_keys/ed25519_test_key.pem") parser.add_argument("--rdct", help="binary R&D certificate file", default=None) - parser.add_argument("--genkey", help="new generated key", default=None) args = parser.parse_args() - # Only generate a key pair, don't sign - if args.genkey: - # Only create a key file, don't sign - generate_key(args.genkey) - print('New key file generated:',args.genkey) - sys.exit(0); - # Check that both signee and signed exist if not args.signee or not args.signed: print("ERROR: Must either provide file names for both signee and signed") @@ -153,11 +82,11 @@ def generate_key(key_file): sys.exit(1) # Issue a warning when signing with testing key - if args.key=='Tools/test_keys/test_keys.json': + if args.key=='Tools/test_keys/ed25519_test_key.pem': print("WARNING: Signing with PX4 test key") # Sign the binary - signed, public_key = sign(args.signee, args.key, args.genkey) + signed, public_key = sign(args.signee, args.key) with open(args.signed, mode='wb') as fs: # Write signed binary @@ -170,4 +99,3 @@ def generate_key(key_file): fs.write(f.read()) except: pass - diff --git a/Tools/px_uploader.Dockerfile b/Tools/px_uploader.Dockerfile new file mode 100644 index 000000000000..728b0d878dc4 --- /dev/null +++ b/Tools/px_uploader.Dockerfile @@ -0,0 +1,33 @@ +FROM python:alpine3.14 + +# run this with something like: +# +# $ docker run --rm -it --network=host --device=/dev/ttyS7:/dev/px4serial px4-fw-updater \ +# --udp-addr=192.168.200.101 \ +# --udp-port=14541 \ +# --port=/dev/px4serial \ +# --baud-bootloader=2000000 \ +# px4_fmu-v5_ssrc.px4 + +# This gets built in environment with somewhat unorthodox paths: +# - The build context is at / +# - The repository itself is mounted in /px4-firmware/ +# - Built firmware files are in /bin/ +# +# ("/" above is relative to GH action runner home dir) +# (see .github/workflows/tiiuae-pixhawk.yaml) + +WORKDIR /firmware + +ENTRYPOINT ["/entrypoint.sh"] + +# dependency of px_uploader.py +RUN pip3 install --user pyserial + +ADD px4-firmware/Tools/px_uploader.py /bin/ +ADD px4-firmware/Tools/px_uploader.entrypoint /entrypoint.sh + +# copy /bin/* -> /firmware/* +ADD bin/ /firmware/ + +ADD px4-firmware/ssrc_config /flight_modes diff --git a/Tools/px_uploader.entrypoint b/Tools/px_uploader.entrypoint new file mode 100755 index 000000000000..1d9b7f87b36b --- /dev/null +++ b/Tools/px_uploader.entrypoint @@ -0,0 +1,13 @@ +#!/bin/sh -e + +aframe="" +if [ ! -z "$AIRFRAME" ]; then + aframe="--airframe ${AIRFRAME}" +fi + +mode="" +if [ ! -z "$FLIGHT_MODE" ]; then + mode="--flightmode ${FLIGHT_MODE}" +fi + +/bin/px_uploader.py --port=/dev/px4serial --udp-addr=192.168.200.101 --baud-flightstack=57600,115200,1000000,2000000 --baud-bootloader=2000000 px4_fmu-v5x_ssrc-*.px4 ssrc_saluki-v1_default-*.px4 ssrc_saluki-v2_default-*.px4 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d479be314837..2cbc3bb309f5 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -212,8 +212,9 @@ class uploader(object): NSH_INIT = bytearray(b'\x0d\x0d\x0d') NSH_REBOOT_BL = b"reboot -b\n" NSH_REBOOT = b"reboot\n" - MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x53\x6b') - MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xcc\x37') + # Reboot commands incl. 4-byte Sp2 header + MAVLINK_REBOOT_ID1 = bytearray(b'\x53\x00\x29\x29\xfe\x21\x72\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x53\x6b') + MAVLINK_REBOOT_ID0 = bytearray(b'\x53\x00\x29\x29\xfe\x21\x45\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xcc\x37') MAX_FLASH_PRGRAM_TIME = 0.001 # Time on an F7 to send SYNC, RESULT from last data in multi RXed @@ -550,8 +551,15 @@ def __verify_v3(self, label, fw): self.__drawProgressBar(label, 1, 100) expect_crc = fw.crc(self.fw_maxsize) self.__send(uploader.GET_CRC + uploader.EOC) - time.sleep(0.5) - report_crc = self.__recv_int() + # wait for maximum 5 seconds for response + for i in range(10): + time.sleep(0.5) + try: + report_crc = self.__recv_int() + break + except: + pass + self.__getSync() if report_crc != expect_crc: print("Expected 0x%x" % expect_crc) @@ -743,10 +751,21 @@ def send_protocol_splitter_format(self, data): self.__send(header_bytes) self.__send(data) - def send_reboot(self, use_protocol_splitter_format=False): + def send_reboot(self, ipaddr, portnum, use_protocol_splitter_format=False): if (not self.__next_baud_flightstack()): return False + print("Attempting reboot on ethernet") + # initialize an UDP socket + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + # send reboot request + s.sendto(self.MAVLINK_REBOOT_ID1, (ipaddr, int(portnum))) + s.sendto(self.MAVLINK_REBOOT_ID0, (ipaddr, int(portnum))) + + # close the socket + s.close() + print("Attempting reboot on %s with baudrate=%d..." % (self.port.port, self.port.baudrate), file=sys.stderr) if "ttyS" in self.port.port: print("If the board does not respond, check the connection to the Flight Controller") @@ -786,6 +805,8 @@ def main(): # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") + parser.add_argument('--udp-addr', action="store", default="192.168.200.100", help="UDP address of PX4 flight controller") + parser.add_argument('--udp-port', action="store", default=14541, help="UDP port of PX4 mavlink") parser.add_argument('--port', action="store", required=True, help="Comma-separated list of serial port(s) to which the FMU may be attached") parser.add_argument('--baud-bootloader', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200) when communicating with bootloader, only required for true serial ports.") parser.add_argument('--baud-flightstack', action="store", default="57600", help="Comma-separated list of baud rate of the serial port (default is 57600) when communicating with flight stack (Mavlink or NSH), only required for true serial ports.") @@ -889,7 +910,7 @@ def main(): except Exception: - if not up.send_reboot(args.use_protocol_splitter_format): + if not up.send_reboot(args.udp_addr, args.udp_port, args.use_protocol_splitter_format): break # wait for the reboot, without we might run into Serial I/O Error 5 diff --git a/Tools/simulation/gazebo/sitl_gazebo b/Tools/simulation/gazebo/sitl_gazebo index 0c8b6fd26fe4..a34e3421096e 160000 --- a/Tools/simulation/gazebo/sitl_gazebo +++ b/Tools/simulation/gazebo/sitl_gazebo @@ -1 +1 @@ -Subproject commit 0c8b6fd26fe45f0952ba3d352f68d114754080bf +Subproject commit a34e3421096e3c5da8983525fc579a5c5abf3d43 diff --git a/Tools/ssrc-sim-tester.sh b/Tools/ssrc-sim-tester.sh new file mode 100755 index 000000000000..bb51c4b8681b --- /dev/null +++ b/Tools/ssrc-sim-tester.sh @@ -0,0 +1,157 @@ +#!/bin/bash + +set -e + +########################################################### +# Argument parsing + +usage() { + echo " +Usage: $(basename "$0") [-h] [-d] [-n count] [-t tag] + -- Run multiple containerized px4_sitl instances in gazebo simulation +Params: + -h Show help text. + -d Delete running simulation + -n Count of px4_sitl instances launched into simulation. Default: 1 + -t Use specific tii-px4-sitl: image for sitl instances. Default: master + -g gzserver image name. Default: ghcr.io/tiiuae/tii-gzserver:tcp + -i gazebo-data image name. Default: ghcr.io/tiiuae/tii-gazebo-data:tcp_test3 +" + exit 0 +} + +check_arg() { + if [ "$(echo $1 | cut -c1)" = "-" ]; then + return 1 + else + return 0 + fi +} + +error_arg() { + echo "$0: option requires an argument -- $1" + usage +} + +delete_current_sim=0 +instance_count=1 +image_tag="master" +gzserver_image=ghcr.io/tiiuae/tii-gzserver:tcp +data_image=ghcr.io/tiiuae/tii-gazebo-data:tcp_test3 + +while getopts "hdn:t:g:i:" opt +do + case $opt in + h) + usage + ;; + d) + delete_current_sim=1 + ;; + n) + check_arg $OPTARG && instance_count=$OPTARG || error_arg $opt + ;; + t) + check_arg $OPTARG && image_tag=$OPTARG || error_arg $opt + ;; + g) + check_arg $OPTARG && image_tag=$OPTARG || error_arg $opt + ;; + i) + check_arg $OPTARG && image_tag=$OPTARG || error_arg $opt + ;; + \?) + usage + ;; + esac +done + + +########################################################### +# Main + +delete_simulation() { + echo "Delete simulation:" + pids=() + dronestr=$(docker ps | grep px4_sitl_drone | awk '{ print $1 }') + docker stop gzserver >/dev/null 2>&1 & + pid=$! + pids+=($pid) + echo " Removing gzserver (pid $pid)" + + if [ "$dronestr" != "" ]; then + readarray -t drone_list <<<"$dronestr" + for dr in ${drone_list[@]} + do + docker stop $dr >/dev/null 2>&1 & + pid=$! + pids+=($pid) + echo " Removing drone '$dr' (pid $pid)" + done + fi + + for p in ${pids[@]} + do + wait $p + echo " pid $p removed." + done + echo "Done." +} + + +if [ $delete_current_sim = 1 ]; then + delete_simulation + exit 0 +fi + + +echo "Run simulation with px4_sitl image: ghcr.io/tiiuae/tii-px4-sitl:${image_tag}" + +if [ ! -d "/tmp/gazebo-data" ]; then + echo "gazebo-data not found, download to '/tmp/gazebo-data'" + docker create --name gdata_cont ${data_image} + docker cp gdata_cont:/gazebo-data /tmp + docker rm gdata_cont + echo "gazebo-data download done." +fi + +if [ "$(docker ps | grep px4_sitl_drone)" != "" ] || [ "$(docker ps | grep gzserver)" != "" ]; then + echo "Old simulation found running, removing..." + delete_simulation +fi + +echo "Start gzserver" +docker run -d --rm --name gzserver --env PX4_SIM_USE_TCP_SERVER=1 -v /tmp/gazebo-data:/data ${gzserver_image} empty.world + +echo "Starting ${instance_count} PX4 instances" +echo + +for i in $(seq 1 $instance_count) +do + drone=px4_sitl_drone${i} + echo "Start px4 instance #${i}/${instance_count}" + docker run -d --rm --name $drone --env PX4_SIM_USE_TCP_SERVER=1 --env PX4_SIM_MODE=ssrc_fog_x ghcr.io/tiiuae/tii-px4-sitl:${image_tag} + echo " Wait 1 sec.." + sleep 1 + drone_ip=$(docker inspect -f '{{ .NetworkSettings.IPAddress }}' ${drone}) + echo " Spawn ${drone} into gazebo simulation instance" + docker exec gzserver /gzserver-api/scripts/spawn-drone.sh ${drone_ip} 12460 4560 5600 ${drone} 0 0 0 0 0 0 + echo +done + +cat << EOF + +To get logs: + $ docker logs -f gzserver + $ docker logs -f px4_sitl_drone1 + $ docker logs -f px4_sitl_drone2 + .. + $ docker logs -f px4_sitl_droneN + +QGC connection: + udp:14550 + +To stop simulation: + $ $0 -d + +EOF diff --git a/Tools/test_keys/ed25519_test_key.pem b/Tools/test_keys/ed25519_test_key.pem new file mode 100644 index 000000000000..ddcf3233a19c --- /dev/null +++ b/Tools/test_keys/ed25519_test_key.pem @@ -0,0 +1,3 @@ +-----BEGIN PRIVATE KEY----- +MC4CAQAwBQYDK2VwBCIEIHNNWX59irCh0NZLlQg6pr7jS0a55udtrB42OvEU8S0V +-----END PRIVATE KEY----- diff --git a/Tools/test_keys/key0.pub b/Tools/test_keys/key0.pub index 86153ac96535..59472655b5f9 100644 --- a/Tools/test_keys/key0.pub +++ b/Tools/test_keys/key0.pub @@ -1,5 +1,7 @@ //Public key to verify signed binaries -0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c, -0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81, -0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb, -0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd, +0x30, 0x2a, 0x30, 0x05, 0x06, 0x03, 0x2b, 0x65, +0x70, 0x03, 0x21, 0x00, 0x4d, 0xb0, 0xc2, 0x01, +0x05, 0x55, 0x2a, 0x3c, 0xd7, 0xfb, 0xaf, 0x5c, +0xba, 0x7a, 0xb0, 0x81, 0x1b, 0x36, 0x63, 0xdb, +0x28, 0x52, 0x5e, 0xdb, 0x14, 0x36, 0xf2, 0x57, +0x8d, 0x02, 0xb7, 0xfd diff --git a/Tools/test_keys/test_keys.json b/Tools/test_keys/test_keys.json deleted file mode 100644 index 127a5be598d5..000000000000 --- a/Tools/test_keys/test_keys.json +++ /dev/null @@ -1 +0,0 @@ -{"date": "Tue Nov 3 13:02:09 2020", "public": "4db0c20105552a3cd7fbaf5cba7ab0811b3663db28525edb1436f2578d02b7fd", "private": "734d597e7d8ab0a1d0d64b95083aa6bee34b46b9e6e76dac1e363af114f12d15"} \ No newline at end of file diff --git a/boards/matek/h743-slim/nuttx-config/nsh/defconfig b/boards/matek/h743-slim/nuttx-config/nsh/defconfig index 59efb9700088..21d66ea115f5 100644 --- a/boards/matek/h743-slim/nuttx-config/nsh/defconfig +++ b/boards/matek/h743-slim/nuttx-config/nsh/defconfig @@ -120,6 +120,35 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_IFCONFIG=n +CONFIG_NSH_DISABLE_IFUPDOWN=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_TELNETD=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/nxp/mr-canhubk3/default.px4board b/boards/nxp/mr-canhubk3/default.px4board index 15e6f15bf03c..277c62de7889 100644 --- a/boards/nxp/mr-canhubk3/default.px4board +++ b/boards/nxp/mr-canhubk3/default.px4board @@ -15,8 +15,6 @@ CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y CONFIG_SYSTEMCMDS_MFT=y -CONFIG_SYSTEMCMDS_MIXER=y -CONFIG_SYSTEMCMDS_MOTOR_TEST=y CONFIG_SYSTEMCMDS_MTD=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_REBOOT=y diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index afaefec9cdaa..366bf7fa0904 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -2,14 +2,14 @@ CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_COMMON_LIGHT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y -CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_RC_INPUT=y -CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_UAVCAN=y CONFIG_EXAMPLES_FAKE_GPS=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y @@ -38,7 +38,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y -CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y @@ -47,7 +46,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_NETMAN=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PERF=y -CONFIG_SYSTEMCMDS_PWM=y CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SERIAL_TEST=y CONFIG_SYSTEMCMDS_TUNE_CONTROL=y diff --git a/boards/nxp/mr-canhubk3/init/rc.board_defaults b/boards/nxp/mr-canhubk3/init/rc.board_defaults index 40fe6fcdfb9d..d9bd59a7fde5 100644 --- a/boards/nxp/mr-canhubk3/init/rc.board_defaults +++ b/boards/nxp/mr-canhubk3/init/rc.board_defaults @@ -15,3 +15,11 @@ param set-default MAV_1_REMOTE_PRT 14550 param set-default MAV_1_UDP_PRT 14550 param set-default SENS_EXT_I2C_PRB 0 + +if param greater -s UAVCAN_ENABLE 0 +then + ifup can0 + ifup can1 + ifup can2 + ifup can3 +fi \ No newline at end of file diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig index 889be6c7bac8..addbed16f70a 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -78,6 +78,7 @@ CONFIG_DEBUG_FEATURES=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 @@ -103,7 +104,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=800 +CONFIG_IDLETHREAD_STACKSIZE=900 CONFIG_INIT_ENTRYPOINT="nsh_main" CONFIG_INIT_STACKSIZE=2944 CONFIG_IOB_NBUFFERS=24 @@ -145,7 +146,10 @@ CONFIG_NET_ARP_IPIN=y CONFIG_NET_ARP_SEND=y CONFIG_NET_BROADCAST=y CONFIG_NET_CAN=y +CONFIG_NET_CAN_EXTID=y +CONFIG_NET_CAN_NOTIFIER=y CONFIG_NET_CAN_RAW_FILTER_MAX=1 +CONFIG_NET_CAN_RAW_TX_DEADLINE=y CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y @@ -155,6 +159,7 @@ CONFIG_NET_TCP=y CONFIG_NET_TCPBACKLOG=y CONFIG_NET_TCP_DELAYED_ACK=y CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_TIMESTAMP=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y CONFIG_NET_UDP_WRITE_BUFFERS=y diff --git a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig index ec03c7169375..3d6145ce179b 100644 --- a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig @@ -133,6 +133,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/debug/defconfig b/boards/px4/fmu-v5/nuttx-config/debug/defconfig index 36a1c9333408..ccbe02e349c0 100644 --- a/boards/px4/fmu-v5/nuttx-config/debug/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/debug/defconfig @@ -180,6 +180,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/mbedtls/defconfig b/boards/px4/fmu-v5/nuttx-config/mbedtls/defconfig index 8e83e6d753e8..1e04f1c5d8e5 100644 --- a/boards/px4/fmu-v5/nuttx-config/mbedtls/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/mbedtls/defconfig @@ -103,9 +103,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 @@ -133,6 +158,7 @@ CONFIG_SCHED_HPWORKPRIORITY=249 CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 @@ -240,5 +266,5 @@ CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index 956db24e60b9..8cb6098edfae 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -89,6 +89,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_EXAMPLES_CALIB_UDELAY=y +CONFIG_DISABLE_PTHREAD=n CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -103,6 +104,7 @@ CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y CONFIG_FS_PROCFS_MAX_TASKS=64 CONFIG_FS_PROCFS_REGISTER=y CONFIG_FS_ROMFS=y +CONFIG_FS_SHMSF=y CONFIG_GRAN=y CONFIG_GRAN_INTR=y CONFIG_HAVE_CXX=y @@ -131,6 +133,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/protected/defconfig b/boards/px4/fmu-v5/nuttx-config/protected/defconfig index 000b2d7d057b..be9da87fc39a 100644 --- a/boards/px4/fmu-v5/nuttx-config/protected/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/protected/defconfig @@ -136,6 +136,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5/nuttx-config/scripts/script.ld index d73a97deb9ce..4aa8307a9b8f 100644 --- a/boards/px4/fmu-v5/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v5/nuttx-config/scripts/script.ld @@ -82,7 +82,7 @@ MEMORY OUTPUT_ARCH(arm) EXTERN(_vectors) -ENTRY(_stext) +ENTRY(_vectors) /* * Ensure that abort() is present in the final object. The exception handling @@ -91,20 +91,39 @@ ENTRY(_stext) EXTERN(abort) EXTERN(_bootdelay_signature) EXTERN(_main_toc) +EXTERN(_main_toc_sig) SECTIONS { - .text : { + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + .toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(0x1000); + } > FLASH_AXIM + + .vectors : { _stext = ABSOLUTE(.); *(.vectors) . = ALIGN(32); + } > FLASH_AXIM + + .text : { /* This signature provides the bootloader with a way to delay booting */ _bootdelay_signature = ABSOLUTE(.); FILL(0xffecc2925d7d05c5) . += 8; - *(.main_toc) *(.text .text.*) *(.fixup) *(.gnu.warning) @@ -180,11 +199,4 @@ SECTIONS } > ITCM_RAM AT > FLASH_AXIM _framfuncs = LOADADDR(.ramfunc); - - /* Start of the image signature. This - * has to be in the end of the image - */ - .signature : { - _boot_signature = ALIGN(4); - } > FLASH_AXIM } diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/toc.ld b/boards/px4/fmu-v5/nuttx-config/scripts/toc.ld new file mode 100644 index 000000000000..b68869ab9fb6 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/scripts/toc.ld @@ -0,0 +1,63 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + /* -64 <- Leave room for the signature */ + progmem (r) : ORIGIN = 0x08008000, LENGTH = 2016K - 64 +} + +OUTPUT_ARCH("arm") + +EXTERN(_main_toc) + +SECTIONS +{ + .toc : { + /* The ToC */ + _toc_start = ABSOLUTE(.); + KEEP(*(.main_toc)); + /* Padd the rest */ + FILL(0xff); + . = 0x1000 - 64; + _toc_end = ABSOLUTE(.); + } > progmem + + /* Start of the ToC signature, appended directly after ToC */ + PROVIDE(_toc_signature = ALIGN(4)); + + .toc_sig (NOLOAD) : { + /* Make a hole for the singature */ + KEEP(*(.main_toc_sig)); + } > progmem + + .app (NOLOAD) : { + /* The application firmware payload */ + _app_start = ABSOLUTE(.); + *(.firmware) + . = ALIGN(4); + _app_end = ABSOLUTE(.); + } > progmem + + /* Start of the payload signature. This has to be in the end of the + * payload and aligned to a 4 byte boundary + */ + PROVIDE(_boot_signature = ALIGN(4)); +} diff --git a/boards/px4/fmu-v5/nuttx-config/ssrc/defconfig b/boards/px4/fmu-v5/nuttx-config/ssrc/defconfig new file mode 100644 index 000000000000..e2205cfaa82d --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/ssrc/defconfig @@ -0,0 +1,273 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y +CONFIG_DISABLE_PTHREAD=n +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART3_TXDMA=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 8563721466c0..d23b51e6b570 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -132,6 +132,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/src/toc.c b/boards/px4/fmu-v5/src/toc.c index 5f3fec31f412..7a1bf1ee53e7 100644 --- a/boards/px4/fmu-v5/src/toc.c +++ b/boards/px4/fmu-v5/src/toc.c @@ -33,21 +33,36 @@ #include /* (Maximum) size of the signature */ + #define SIGNATURE_SIZE 64 -/* Boot image starts at _vectors and ends at - * the beginning of signature -*/ +/* ToC area boundaries */ +extern const uintptr_t _toc_start; +extern const uintptr_t _toc_end; + +#define TOC_ADDR &_toc_start +#define TOC_END ((const void *)&_toc_end) + +/* ToC signature */ +extern const uintptr_t _toc_signature; -extern uint32_t _vectors[]; -extern const int *_boot_signature; +#define TOCSIG_ADDR ((const void *)&_toc_signature) +#define TOCSIG_END ((const void *)((const uint8_t *)TOCSIG_ADDR+SIGNATURE_SIZE)) -#define BOOT_ADDR _vectors -#define BOOT_END ((const void *)&_boot_signature) +/* Boot image starts at __start and ends at + * the beginning of signature, but for protected/kernel mode we don't know + * their locations. Assume binary file start and binary file end ? +*/ +extern const uintptr_t _app_start; +extern const uintptr_t _app_end; + +#define BOOT_ADDR &_app_start +#define BOOT_END ((const void *)&_app_end) /* Boot signature start and end are defined by the * signature definition below */ +extern const uintptr_t _boot_signature; #define BOOTSIG_ADDR ((const void *)&_boot_signature) #define BOOTSIG_END ((const void *)((const uint8_t *)BOOTSIG_ADDR+SIGNATURE_SIZE)) @@ -64,13 +79,19 @@ extern const int *_boot_signature; /* The table of contents */ -IMAGE_MAIN_TOC(4) = { +IMAGE_MAIN_TOC(6) = { {TOC_START_MAGIC, TOC_VERSION}, { - {"BOOT", BOOT_ADDR, BOOT_END, 0, 1, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE}, + {"TOC", TOC_ADDR, TOC_END, 0, 1, 0, 0, TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG0", TOCSIG_ADDR, TOCSIG_END, 0, 0, 0, 0, 0}, + {"BOOT", BOOT_ADDR, BOOT_END, 0, 3, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE}, {"SIG1", BOOTSIG_ADDR, BOOTSIG_END, 0, 0, 0, 0, 0}, - {"RDCT", RDCT_ADDR, RDCT_END, 0, 3, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, + {"RDCT", RDCT_ADDR, RDCT_END, 0, 5, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, {"RDSG", RDCTSIG_ADDR, RDCTSIG_END, 0, 0, 0, 0, 0}, }, TOC_END_MAGIC }; + +/* Define a signature area, just for sizing the ToC area */ + +const char _main_toc_sig[SIGNATURE_SIZE] __attribute__((section(".main_toc_sig"))); diff --git a/boards/px4/fmu-v5/ssrc.px4board b/boards/px4/fmu-v5/ssrc.px4board new file mode 100644 index 000000000000..e3edc876c45d --- /dev/null +++ b/boards/px4/fmu-v5/ssrc.px4board @@ -0,0 +1,10 @@ +CONFIG_BOARD_CRYPTO=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_MODULES_MICRORTPS_BRIDGE=y +CONFIG_DRIVERS_PROTOCOL_SPLITTER=y + +# Test keys to help local building +# These can be overridden in CI with environment variables pointing to real keys +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index ba812e4b7bfd..b9a040d9a3a0 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -19,3 +19,5 @@ param set-default SENS_EN_INA226 1 param set-default SYS_USE_IO 1 safety_button start + +set LOGGER_BUF 8 diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index dad84376ce73..0d69e36794f6 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -8,3 +8,12 @@ then # Start MAVLink on the UART connected to the mission computer mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z fi + +# Start MC flight control mavlink to ethernet interface +mavlink start -c 192.168.200.100 -u 14541 -o 14540 -r 2000000 -x + +# Start uXRCE-DDS client on UDP +microdds_client start -t udp -h 192.168.200.100 -r 2019 -p 2020 + +# Start MC maintenance mavlink to ethernet interface +#mavlink start -c 192.168.200.100 -u 14543 -o 14542 -r 2000000 -m magic -x diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 4f7c1d078de5..4910c2ff05fb 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -70,7 +70,7 @@ then icm20602 -R 10 -s start # Internal magnetometer on I2c - bmm150 -I start + #bmm150 -I start else #SKYNODE base fmu board orientation diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 691db4b31c96..0e83cee57837 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -92,6 +92,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_GPIO=y CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_PTHREAD=n CONFIG_ETH0_PHY_LAN8742A=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -108,6 +109,7 @@ CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y CONFIG_FS_PROCFS_MAX_TASKS=64 CONFIG_FS_PROCFS_REGISTER=y CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y CONFIG_GRAN=y CONFIG_GRAN_INTR=y CONFIG_HAVE_CXX=y @@ -135,6 +137,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 CONFIG_NET=y CONFIG_NETDB_DNSCLIENT=y CONFIG_NETDB_DNSCLIENT_ENTRIES=8 @@ -167,6 +170,37 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_IFCONFIG=n +CONFIG_NSH_DISABLE_IFUPDOWN=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_TELNETD=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld index 8d5a78da6bfc..4aa8307a9b8f 100644 --- a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld @@ -82,7 +82,7 @@ MEMORY OUTPUT_ARCH(arm) EXTERN(_vectors) -ENTRY(_stext) +ENTRY(_vectors) /* * Ensure that abort() is present in the final object. The exception handling @@ -90,14 +90,34 @@ ENTRY(_stext) */ EXTERN(abort) EXTERN(_bootdelay_signature) -EXTERN(board_get_manifest) +EXTERN(_main_toc) +EXTERN(_main_toc_sig) SECTIONS { - .text : { + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + .toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(0x1000); + } > FLASH_AXIM + + .vectors : { _stext = ABSOLUTE(.); *(.vectors) . = ALIGN(32); + } > FLASH_AXIM + + .text : { /* This signature provides the bootloader with a way to delay booting */ @@ -127,14 +147,6 @@ SECTIONS _einit = ABSOLUTE(.); } > FLASH_AXIM - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > FLASH_AXIM .ARM.extab : { *(.ARM.extab*) diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/toc.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/toc.ld new file mode 100644 index 000000000000..b68869ab9fb6 --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/toc.ld @@ -0,0 +1,63 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + /* -64 <- Leave room for the signature */ + progmem (r) : ORIGIN = 0x08008000, LENGTH = 2016K - 64 +} + +OUTPUT_ARCH("arm") + +EXTERN(_main_toc) + +SECTIONS +{ + .toc : { + /* The ToC */ + _toc_start = ABSOLUTE(.); + KEEP(*(.main_toc)); + /* Padd the rest */ + FILL(0xff); + . = 0x1000 - 64; + _toc_end = ABSOLUTE(.); + } > progmem + + /* Start of the ToC signature, appended directly after ToC */ + PROVIDE(_toc_signature = ALIGN(4)); + + .toc_sig (NOLOAD) : { + /* Make a hole for the singature */ + KEEP(*(.main_toc_sig)); + } > progmem + + .app (NOLOAD) : { + /* The application firmware payload */ + _app_start = ABSOLUTE(.); + *(.firmware) + . = ALIGN(4); + _app_end = ABSOLUTE(.); + } > progmem + + /* Start of the payload signature. This has to be in the end of the + * payload and aligned to a 4 byte boundary + */ + PROVIDE(_boot_signature = ALIGN(4)); +} diff --git a/boards/px4/fmu-v5x/nuttx-config/ssrc/defconfig b/boards/px4/fmu-v5x/nuttx-config/ssrc/defconfig new file mode 100644 index 000000000000..89567bdb8784 --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/ssrc/defconfig @@ -0,0 +1,316 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_FS_ANONMAP is not set +# CONFIG_FS_PROCFS_EXCLUDE_MEMINFO is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NET_TCP_RECV_PACK is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_ARP is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_DMESG is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_IFUPDOWN is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_UPTIME is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5x/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0033 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5X.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_GPIO=y +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_ETH0_PHY_LAN8742A=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DRIPADDR=0xc0a8c864 +CONFIG_NETINIT_IPADDR=0xc0a8c865 +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_ETH_PKTSIZE=1518 +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_NACTIVESOCKETS=16 +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_NOTIFIER=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_NOTIFIER=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC2_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32F7_ADC1=y +CONFIG_STM32F7_ADC3=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_ETHMAC=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PHYADDR=0 +CONFIG_STM32F7_PHYSR=31 +CONFIG_STM32F7_PHYSR_100MBPS=0x8 +CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10 +CONFIG_STM32F7_PHYSR_MODE=0x10 +CONFIG_STM32F7_PHYSR_SPEED=0x8 +CONFIG_STM32F7_PHY_POLLING=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC2=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI2_DMA=y +CONFIG_STM32F7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32F7_SPI3=y +CONFIG_STM32F7_SPI3_DMA=y +CONFIG_STM32F7_SPI3_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM9=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART5=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=3000 +CONFIG_UART5_TXDMA=y +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index 684b11736eda..e07012a91592 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -42,6 +42,7 @@ add_library(drivers_board spi.cpp timer_config.cpp usb.c + toc.c ) add_dependencies(drivers_board platform_gpio_mcp23009) diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 281c769f2646..096f344e9c91 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -43,6 +43,7 @@ * Included Files ****************************************************************************************************/ +#include #include #include #include diff --git a/boards/px4/fmu-v5x/src/toc.c b/boards/px4/fmu-v5x/src/toc.c new file mode 100644 index 000000000000..ba9ff4d033bb --- /dev/null +++ b/boards/px4/fmu-v5x/src/toc.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. 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. + * + ****************************************************************************/ +#include + +/* (Maximum) size of the signature */ + +#define SIGNATURE_SIZE 64 + +/* ToC area boundaries */ +extern const uintptr_t _toc_start; +extern const uintptr_t _toc_end; + +#define TOC_ADDR &_toc_start +#define TOC_END ((const void *)&_toc_end) + +/* ToC signature */ +extern const uintptr_t _toc_signature; + +#define TOCSIG_ADDR ((const void *)&_toc_signature) +#define TOCSIG_END ((const void *)((const uint8_t *)TOCSIG_ADDR+SIGNATURE_SIZE)) + +/* Boot image starts at __start and ends at + * the beginning of signature, but for protected/kernel mode we don't know + * their locations. Assume binary file start and binary file end ? +*/ +extern const uintptr_t _app_start; +extern const uintptr_t _app_end; + +#define BOOT_ADDR &_app_start +#define BOOT_END ((const void *)&_app_end) + +/* Boot signature start and end are defined by the + * signature definition below +*/ +extern const uintptr_t _boot_signature; + +#define BOOTSIG_ADDR ((const void *)&_boot_signature) +#define BOOTSIG_END ((const void *)((const uint8_t *)BOOTSIG_ADDR+SIGNATURE_SIZE)) + +/* RD certifcate may follow boot signature */ + +#define RDCT_ADDR BOOTSIG_END +#define RDCT_END ((const void *)((const uint8_t*)BOOTSIG_END+sizeof(image_cert_t))) + +/* RD certificate signature follows the certificate */ + +#define RDCTSIG_ADDR RDCT_END +#define RDCTSIG_END ((const void *)((const uint8_t*)RDCTSIG_ADDR+SIGNATURE_SIZE)) + +/* The table of contents */ + +IMAGE_MAIN_TOC(6) = { + {TOC_START_MAGIC, TOC_VERSION}, + { + {"TOC", TOC_ADDR, TOC_END, 0, 1, 0, 0, TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG0", TOCSIG_ADDR, TOCSIG_END, 0, 0, 0, 0, 0}, + {"BOOT", BOOT_ADDR, BOOT_END, 0, 3, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG1", BOOTSIG_ADDR, BOOTSIG_END, 0, 0, 0, 0, 0}, + /* + {"RDCT", RDCT_ADDR, RDCT_END, 0, 5, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, + {"RDSG", RDCTSIG_ADDR, RDCTSIG_END, 0, 0, 0, 0, 0}, + */ + }, + TOC_END_MAGIC +}; + +/* Define a signature area, just for sizing the ToC area */ + +const char _main_toc_sig[SIGNATURE_SIZE] __attribute__((section(".main_toc_sig"))); diff --git a/boards/px4/fmu-v5x/ssrc.px4board b/boards/px4/fmu-v5x/ssrc.px4board new file mode 100644 index 000000000000..3a6dd5367be7 --- /dev/null +++ b/boards/px4/fmu-v5x/ssrc.px4board @@ -0,0 +1,38 @@ +CONFIG_BOARD_CRYPTO=y +CONFIG_COMMON_OSD=n +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_MODULES_MICRORTPS_BRIDGE=y +CONFIG_MODULES_MICRODDS_CLIENT=y +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SIH=n +CONFIG_SYSTEMCMDS_NETMAN=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_CAMERA_FEEDBACK=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SIH=n +CONFIG_SYSTEMCMDS_I2CDETECT=n +CONFIG_SYSTEMCMDS_NETMAN=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_DRIVERS_ROBOCLAW=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_COMMON_OPTICAL_FLOW=n +CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n +CONFIG_DRIVERS_POWER_MONITOR_INA228=n +CONFIG_DRIVERS_POWER_MONITOR_INA238=n + +# Test keys to help local building +# These can be overridden in CI with environment variables pointing to real keys +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY2="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 826b26c2e607..e4761a78e55c 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -74,7 +74,7 @@ if ver hwtypecmp V6X002001 then rm3100 -I -b 4 start else - bmm150 -I start + #bmm150 -I start fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index ff8dda3a5cb1..3462599d1bca 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -5,6 +5,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y diff --git a/boards/ssrc/icicle/amp.px4board b/boards/ssrc/icicle/amp.px4board new file mode 100644 index 000000000000..ecdf2eec99dc --- /dev/null +++ b/boards/ssrc/icicle/amp.px4board @@ -0,0 +1 @@ +CONFIG_DRIVERS_PROTOCOL_SPLITTER=n \ No newline at end of file diff --git a/boards/ssrc/icicle/bootloader.px4board b/boards/ssrc/icicle/bootloader.px4board new file mode 100644 index 000000000000..36034e3299d2 --- /dev/null +++ b/boards/ssrc/icicle/bootloader.px4board @@ -0,0 +1,7 @@ +CONFIG_BOARD_TOOLCHAIN="riscv64-unknown-elf" +CONFIG_BOARD_ARCHITECTURE="rv64gc" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CRYPTO=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" diff --git a/boards/ssrc/icicle/bootloader_readme.txt b/boards/ssrc/icicle/bootloader_readme.txt new file mode 100644 index 000000000000..ebfd0d3dadb5 --- /dev/null +++ b/boards/ssrc/icicle/bootloader_readme.txt @@ -0,0 +1,13 @@ +1. build icicle bootloader: + +- Set PATH to point to a working riscv64-unknown-elf-gcc +- $ make ssrc_icicle_bootloader + +2. flash the icicle bootloader to eNVM + + + +$ cp /build/ssrc_icicle_bootloader/ssrc_icicle_bootloader.elf . +$ sudo SC_INSTALL_DIR= FPGENPROG=/Libero/bin64/fpgenprog java -jar $SC_INSTALL_DIR/extras/mpfs/mpfsBootmodeProgrammer.jar --bootmode 1 ssrc_icicle_bootloader.elf + +3. use px_uploader.py to flash the px4 binary diff --git a/boards/ssrc/icicle/default.px4board b/boards/ssrc/icicle/default.px4board new file mode 100644 index 000000000000..42c677fa5f31 --- /dev/null +++ b/boards/ssrc/icicle/default.px4board @@ -0,0 +1,87 @@ +CONFIG_BOARD_TOOLCHAIN="riscv64-unknown-elf" +CONFIG_BOARD_ARCHITECTURE="rv64gc" +CONFIG_BOARD_CRYPTO=y +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PWM_ESC=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y + +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY2="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/ssrc/icicle/firmware.prototype b/boards/ssrc/icicle/firmware.prototype new file mode 100644 index 000000000000..c370b014daf3 --- /dev/null +++ b/boards/ssrc/icicle/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1500, + "magic": "Icicle", + "description": "Firmware for the Icicle board", + "image": "", + "build_time": 0, + "summary": "SSRC/Icicle", + "version": "0.1", + "image_size": 0, + "image_maxsize": 5242880, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ssrc/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN.job b/boards/ssrc/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN.job new file mode 100644 index 000000000000..80f45aacebd0 Binary files /dev/null and b/boards/ssrc/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN.job differ diff --git a/boards/ssrc/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN_job.digest b/boards/ssrc/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN_job.digest new file mode 100644 index 000000000000..b7585fad8255 --- /dev/null +++ b/boards/ssrc/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN_job.digest @@ -0,0 +1,5 @@ +BITS component bitstream digest: aee355e728bed157183d65b414803ca9422e59d42b06365e61ddf1ed5ec41d74 +Security component bitstream digest: d0b05cff123c1dc5e8d169e7592aa10eb08c57ebe80322cbc8e674e466523457 +Fabric component bitstream digest: c86c1db8daa0326a234a8420929ae46b4ecf9442121b36faf3c986486efb01df +sNVM component bitstream digest: aff56888b71e479e97ce28d42cd956ce55c4e225da7fafe92e84c8317e8f043f +EOB component bitstream digest: c7efe4578036a5b394aed06c53b217533bca0b865b16069e1901c282a8d782f5 diff --git a/boards/ssrc/icicle/fpga/icicle/README.md b/boards/ssrc/icicle/fpga/icicle/README.md new file mode 100644 index 000000000000..c2f14b8e6577 --- /dev/null +++ b/boards/ssrc/icicle/fpga/icicle/README.md @@ -0,0 +1 @@ +# FPGA configurations for flying with Microchip PolarFire SoC Icicle Kit + TII connector board diff --git a/boards/ssrc/icicle/include/board_type.h b/boards/ssrc/icicle/include/board_type.h new file mode 100644 index 000000000000..8447b41c2c6d --- /dev/null +++ b/boards/ssrc/icicle/include/board_type.h @@ -0,0 +1,16 @@ +#ifndef _BOARD_TYPE_H_ +#define _BOARD_TYPE_H_ + +#include + +#define BOARD_TYPE 1500 + +#define BOOTLOADER_SIGNING_ALGORITHM CRYPTO_ED25519 +#define BOOTLOADER_VERIFY_UBOOT 0 +#define BOOTLOADER_BOOT_HART_1 0 +#define BOOTLOADER_BOOT_HART_4 0 + +#define TOC_VERIFICATION_KEY 0 +#define BOOT_VERIFICATION_KEY 0 + +#endif diff --git a/boards/ssrc/icicle/init/rc.board_defaults b/boards/ssrc/icicle/init/rc.board_defaults new file mode 100644 index 000000000000..fbb4c5527296 --- /dev/null +++ b/boards/ssrc/icicle/init/rc.board_defaults @@ -0,0 +1,15 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +set OUTPUT_MODE pwm_esc +set AUX_MODE none + +pwm_esc start + +param set-default COM_PREARM_MODE 1 +param set-default CBRK_IO_SAFETY 0 +param set-default SYS_DM_BACKEND 1 + +safety_button start diff --git a/boards/ssrc/icicle/init/rc.board_mavlink b/boards/ssrc/icicle/init/rc.board_mavlink new file mode 100644 index 000000000000..7c9d0174c108 --- /dev/null +++ b/boards/ssrc/icicle/init/rc.board_mavlink @@ -0,0 +1,5 @@ +#!/bin/sh +# +# PX4 FMUv5 specific board MAVLink startup script. +#------------------------------------------------------------------------------ + diff --git a/boards/ssrc/icicle/init/rc.board_paths b/boards/ssrc/icicle/init/rc.board_paths new file mode 100644 index 000000000000..000d68e186f6 --- /dev/null +++ b/boards/ssrc/icicle/init/rc.board_paths @@ -0,0 +1 @@ +set PARAM_FILE /fs/lfs/params diff --git a/boards/ssrc/icicle/init/rc.board_sensors b/boards/ssrc/icicle/init/rc.board_sensors new file mode 100644 index 000000000000..51eb6551b7aa --- /dev/null +++ b/boards/ssrc/icicle/init/rc.board_sensors @@ -0,0 +1,24 @@ +#!/bin/sh +# +# Icicle specific board sensors init +#------------------------------------------------------------------------------ + +#board_adc start + +# Internal I2C bus +bmp388 -I -a 0x76 -f 400 start +bmp388 -I -a 0x77 -f 400 start +bmm150 -I -a 0x10 -f 400 -R 14 start +#bmm150 -I -a 0x11 -f 400 -R 14 start +ads1115 -I start + +# External I2C +ll40ls -X -f 400 start + +# Internal SPI bus +icm42688p -s -f 10000 -R 10 start +icm20649 -s -f 7000 -R 6 start + +# External compass with lights, safety button, and buzzer +ist8310 -X -a 0x0e -b 2 -f 400 -R 10 start +rgbled_ncp5623c -X -b 2 -f 400 start diff --git a/boards/ssrc/icicle/minimal.px4board b/boards/ssrc/icicle/minimal.px4board new file mode 100644 index 000000000000..9154b4c0ea12 --- /dev/null +++ b/boards/ssrc/icicle/minimal.px4board @@ -0,0 +1,83 @@ +CONFIG_BOARD_TOOLCHAIN="riscv64-unknown-elf" +CONFIG_BOARD_ARCHITECTURE="rv64gc" +CONFIG_BOARD_CRYPTO=n +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS2" +CONFIG_BOARD_ROMFSROOT="minimal_romfs" +CONFIG_COMMON_BAROMETERS=n +CONFIG_DRIVERS_BATT_SMBUS=n +CONFIG_COMMON_DISTANCE_SENSOR=n +CONFIG_DRIVERS_GPS=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=n +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_BAROMETER_BMP388=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_SW_CRYPTO=n +CONFIG_DRIVERS_STUB_KEYSTORE=n +CONFIG_COMMON_LIGHT=n +CONFIG_COMMON_MAGNETOMETER=n +CONFIG_COMMON_OPTICAL_FLOW=n +CONFIG_DRIVERS_PWM_ESC=n +CONFIG_DRIVERS_RC_INPUT=n +CONFIG_DRIVERS_SAFETY_BUTTON=n +CONFIG_DRIVERS_SMART_BATTERY_BATMON=n +CONFIG_DRIVERS_TONE_ALARM=n +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_COMMON_TELEMETRY=n +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6 +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_BATTERY_STATUS=n +CONFIG_MODULES_COMMANDER=n +CONFIG_MODULES_DATAMAN=n +CONFIG_MODULES_EKF2=n +CONFIG_MODULES_EVENTS=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL_L1=n +CONFIG_MODULES_GYRO_CALIBRATION=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LAND_DETECTOR=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_LOAD_MON=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_LOGGER=n +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=n +CONFIG_MODULES_MAVLINK=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_NAVIGATOR=n +CONFIG_MODULES_RC_UPDATE=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SENSORS=n +CONFIG_MODULES_SIH=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_VMOUNT=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_MICRORTPS_BRIDGE=n +CONFIG_DRIVERS_PROTOCOL_SPLITTER=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_ESC_CALIB=n +CONFIG_SYSTEMCMDS_I2CDETECT=n +CONFIG_SYSTEMCMDS_LED_CONTROL=n +CONFIG_SYSTEMCMDS_MFT=n +CONFIG_SYSTEMCMDS_MIXER=n +CONFIG_SYSTEMCMDS_MOTOR_RAMP=n +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_MTD=n +CONFIG_SYSTEMCMDS_NSHTERM=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=n +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n +CONFIG_SYSTEMCMDS_TUNE_CONTROL=n +CONFIG_EXAMPLES_FAKE_GPS=n diff --git a/boards/ssrc/icicle/nuttx-config/Kconfig b/boards/ssrc/icicle/nuttx-config/Kconfig new file mode 100644 index 000000000000..ae2bf31307d0 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/Kconfig @@ -0,0 +1,4 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# diff --git a/boards/ssrc/icicle/nuttx-config/amp/defconfig b/boards/ssrc/icicle/nuttx-config/amp/defconfig new file mode 100644 index 000000000000..2badf09ebd29 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/amp/defconfig @@ -0,0 +1,165 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_FS_ROMFS_CACHE_NODE is not set +CONFIG_ALLOW_BSD_COMPONENTS=y +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ssrc/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=54000 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_ZERO=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EE24XX_FREQUENCY=400000 +CONFIG_EEPROM=y +CONFIG_FAT_DIRECT_RETRY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_EXCLUDE_ENVIRON=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_DRIVER=y +CONFIG_I2C_EE_24XX=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=8192 +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_PERROR_STDOUT=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIBM=y +CONFIG_M25P_SUBSECTOR_ERASE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MPFS_CH1_VRING0_DESC_ADDR=0xADC00000 +CONFIG_MPFS_CH1_VRING1_DESC_ADDR=0xADC08000 +CONFIG_MPFS_CH1_VRING_SHMEM_ADDR=0xADE00000 +CONFIG_MPFS_COREPWM0=y +CONFIG_MPFS_COREPWM0_PWMCLK=62500000 +CONFIG_MPFS_COREPWM1=y +CONFIG_MPFS_COREPWM1_NCHANNELS=1 +CONFIG_MPFS_COREPWM1_PWMCLK=62500000 +CONFIG_MPFS_ENABLE_DPFPU=y +CONFIG_MPFS_HAVE_COREPWM=y +CONFIG_MPFS_I2C0=y +CONFIG_MPFS_I2C1=y +CONFIG_MPFS_IHC_CLIENT=y +CONFIG_MPFS_IHC_NUTTX_ON_HART1=0 +CONFIG_MPFS_IHC_NUTTX_ON_HART2=1 +CONFIG_MPFS_IHC_NUTTX_ON_HART3=0 +CONFIG_MPFS_IHC_NUTTX_ON_HART4=0 +CONFIG_MPFS_IHC_LINUX_ON_HART1=0 +CONFIG_MPFS_IHC_LINUX_ON_HART2=0 +CONFIG_MPFS_IHC_LINUX_ON_HART3=1 +CONFIG_MPFS_IHC_LINUX_ON_HART4=0 +CONFIG_MPFS_SPI0=y +CONFIG_MPFS_SPI1=y +CONFIG_MPFS_UART0=y +CONFIG_MPFS_UART1=y +CONFIG_MPFS_UART4=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_M25P=y +CONFIG_MTD_PARTITION=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_RPMSG=y +CONFIG_NET_RPMSG_DRV=y +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_TCP=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_NOTIFIER=y +CONFIG_NET_TUN=y +CONFIG_NET_TUN_PKTSIZE=576 +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_NOTIFIER=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_PROMPT_STRING="icicle> " +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OPENAMP=y +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTABLE_PARTITION=y +CONFIG_PTHREAD_STACK_MIN=1024 +CONFIG_PWM=y +CONFIG_PWM_NCHANNELS=8 +CONFIG_RAM_SIZE=2097152 +CONFIG_RAM_START=0xAD000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RPTUN=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=4096 +CONFIG_SCHED_WAITPID=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI_DRIVER=y +CONFIG_STACK_COLORATION=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_NSH_STACKSIZE=2304 +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_UART1_RXBUFSIZE=600 +CONFIG_UART1_TXBUFSIZE=1500 diff --git a/boards/ssrc/icicle/nuttx-config/bootloader/defconfig b/boards/ssrc/icicle/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..45ecb64ee65f --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/bootloader/defconfig @@ -0,0 +1,97 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_ARCH_FPU is not set +CONFIG_ALLOW_BSD_COMPONENTS=y +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ssrc/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=12500 +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_COMPOSITE=y +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_COMPOSITE_IAD=y +CONFIG_COMPOSITE_PRODUCTID=0x05e1 +CONFIG_COMPOSITE_PRODUCTSTR="SSRC Icicle v1.0" +CONFIG_COMPOSITE_VENDORID=0x16c0 +CONFIG_COMPOSITE_VENDORSTR="SSRC" +CONFIG_DEBUG_FEATURES=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FAT_DIRECT_RETRY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FS_FAT=y +CONFIG_FS_LARGEFILE=y +CONFIG_GPT_PARTITION=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=8096 +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBM=y +CONFIG_MBR_PARTITION=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MPFS_BOOTLOADER=y +CONFIG_MPFS_DDR_INIT=y +#CONFIG_MPFS_DDR_TYPE_LPDDR4=y +CONFIG_MPFS_EMMCSD=y +CONFIG_MPFS_EMMCSD_MUX_EMMC=y +CONFIG_MPFS_HART1_ENTRYPOINT=0x80200000 +CONFIG_MPFS_HART1_SBI=y +CONFIG_MPFS_HART2_ENTRYPOINT=0xACA00000 +CONFIG_MPFS_HART3_ENTRYPOINT=0x80200000 +CONFIG_MPFS_HART3_SBI=y +CONFIG_MPFS_HART4_ENTRYPOINT=0x80200000 +CONFIG_MPFS_HART4_SBI=y +CONFIG_MPFS_IHC_NUTTX_ON_HART1=0 +CONFIG_MPFS_IHC_NUTTX_ON_HART2=1 +CONFIG_MPFS_IHC_NUTTX_ON_HART3=0 +CONFIG_MPFS_IHC_NUTTX_ON_HART4=0 +CONFIG_MPFS_IHC_LINUX_ON_HART1=0 +CONFIG_MPFS_IHC_LINUX_ON_HART2=0 +CONFIG_MPFS_IHC_LINUX_ON_HART3=1 +CONFIG_MPFS_IHC_LINUX_ON_HART4=0 +CONFIG_MPFS_IHC_SBI=y +CONFIG_MPFS_OPENSBI=y +CONFIG_MPFS_UART0=y +CONFIG_OPENSBI=y +CONFIG_OPENSBI_DOMAINS=y +CONFIG_PTABLE_PARTITION=y +CONFIG_RAM_SIZE=1048576 +CONFIG_RAM_START=0x08000000 +CONFIG_RAW_BINARY=y +CONFIG_SCHED_HPWORK=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_COMPOSITE=y +CONFIG_TASK_NAME_SIZE=20 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_USBDEV=y +CONFIG_USBDEV_COMPOSITE=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USBMSC=y +CONFIG_USBMSC_COMPOSITE=y +CONFIG_USBMSC_IFNOBASE=2 +CONFIG_USBMSC_NRDREQS=16 +CONFIG_USBMSC_NWRREQS=16 +CONFIG_USBMSC_WRMULTIPLE=y +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ssrc/icicle/nuttx-config/include/board.h b/boards/ssrc/icicle/nuttx-config/include/board.h new file mode 100644 index 000000000000..608f65acb16d --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/include/board.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/include/board.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_RISCV_ICICLE_MPFS_INCLUDE_BOARD_H +#define __BOARDS_RISCV_ICICLE_MPFS_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "mpfs_gpio.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Clocking TODO: */ + +#define MPFS_MSS_EXT_SGMII_REF_CLK (125000000UL) +#define MPFS_MSS_COREPLEX_CPU_CLK (600000000UL) +#define MPFS_MSS_SYSTEM_CLK (600000000UL) +#define MPFS_MSS_RTC_TOGGLE_CLK (1000000UL) +#define MPFS_MSS_AXI_CLK (300000000UL) +#define MPFS_MSS_APB_AHB_CLK (150000000UL) +#define MPFS_FPGA_BCLK (3000000UL) + +/* LED definitions **********************************************************/ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_LED4 3 +#define BOARD_NLEDS 4 + +#define BOARD_LED_ORANGE1 BOARD_LED1 +#define BOARD_LED_ORANGE2 BOARD_LED2 +#define BOARD_LED_RED1 BOARD_LED3 +#define BOARD_LED_RED2 BOARD_LED4 + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 + LED2 */ +#define LED_STACKCREATED 3 /* LED3 */ +#define LED_INIRQ 4 /* LED3 + LED1 */ +#define LED_SIGNAL 5 /* LED3 + LED2 */ +#define LED_ASSERTION 6 /* LED3 + LED2 + LED1 */ +#define LED_PANIC 7 /* LED4 */ + +/* Button definitions *******************************************************/ + +/* The Icicle supports 3 buttons: */ + +#define BUTTON_USER1 0 +#define BUTTON_USER2 1 +#define BUTTON_USER3 2 +#define NUM_BUTTONS 3 +#define BUTTON_USER1_BIT (1 << BUTTON_USER1) +#define BUTTON_USER2_BIT (1 << BUTTON_USER2) +#define BUTTON_USER3_BIT (1 << BUTTON_USER3) + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_boardinitialize + ****************************************************************************/ + +void mpfs_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif +#endif /* __ASSEMBLY__ */ +#endif /* __BOARDS_RISCV_ICICLE_MPFS_INCLUDE_BOARD_H */ diff --git a/boards/ssrc/icicle/nuttx-config/include/board_liberodefs.h b/boards/ssrc/icicle/nuttx-config/include/board_liberodefs.h new file mode 100644 index 000000000000..e57919567798 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/include/board_liberodefs.h @@ -0,0 +1,617 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/include/board_liberodefs.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_LIBERODEFS_H +#define __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_LIBERODEFS_H + +/* These are board specific constants provided by the vendor. Values have + * been synced from hart-software-services (HSS) with the tag: 2021.08. + * Different memories have their own subset of defines. + */ + +#define LIBERO_SETTING_MSS_EXT_SGMII_REF_CLK 125000000 +#define LIBERO_SETTING_MSS_RTC_TOGGLE_CLK 1000000 +#define LIBERO_SETTING_MSS_ENVM_CR 0x40050006 +#define LIBERO_SETTING_MSS_CLOCK_CONFIG_CR 0x00000024 +#define LIBERO_SETTING_MSS_MSSCLKMUX 0x00000003 +#define LIBERO_SETTING_MSS_PLL_CKMUX 0x00000155 +#define LIBERO_SETTING_MSS_BCLKMUX 0x00000208 +#define LIBERO_SETTING_MSS_FMETER_ADDR 0x00000000 +#define LIBERO_SETTING_MSS_FMETER_DATAW 0x00000000 +#define LIBERO_SETTING_MSS_FMETER_DATAR 0x00000000 +#define LIBERO_SETTING_MSS_PLL_REF_FB 0x00000500 +#define LIBERO_SETTING_MSS_PLL_CTRL 0x01000037 +#define LIBERO_SETTING_MSS_PLL_DIV_0_1 0x01000200 +#define LIBERO_SETTING_MSS_PLL_DIV_2_3 0x0f000600 +#define LIBERO_SETTING_MSS_SSCG_REG_0 0x00000000 +#define LIBERO_SETTING_MSS_SSCG_REG_1 0x00000000 +#define LIBERO_SETTING_MSS_SSCG_REG_2 0x000000c0 +#define LIBERO_SETTING_MSS_SSCG_REG_3 0x00000001 +#define LIBERO_SETTING_MSS_PLL_FRACN 0x00000000 +#define LIBERO_SETTING_MSS_PLL_CTRL2 0x00001020 +#define LIBERO_SETTING_MSS_PLL_PHADJ 0x00004003 + +#define LIBERO_SETTING_CH0_CNTL 0x37f07770 +#define LIBERO_SETTING_CH1_CNTL 0x37f07770 +#define LIBERO_SETTING_SPARE_CNTL 0xff000000 + +#define LIBERO_SETTING_SGMII_SGMII_CLKMUX 0x00000005 +#define LIBERO_SETTING_SGMII_CLK_XCVR 0x00002c30 +#define LIBERO_SETTING_SGMII_REFCLKMUX 0x00000005 +#define LIBERO_SETTING_SGMII_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_SGMII_PLL_REF_FB 0x00000100 +#define LIBERO_SETTING_SGMII_PLL_DIV_0_1 0x01000100 +#define LIBERO_SETTING_SGMII_PLL_DIV_2_3 0x01000100 +#define LIBERO_SETTING_SGMII_PLL_CTRL2 0x00001020 +#define LIBERO_SETTING_SGMII_PLL_FRACN 0x00000000 +#define LIBERO_SETTING_SGMII_SSCG_REG_0 0x00000000 +#define LIBERO_SETTING_SGMII_SSCG_REG_1 0x00000000 +#define LIBERO_SETTING_SGMII_SSCG_REG_2 0x00000014 +#define LIBERO_SETTING_SGMII_SSCG_REG_3 0x00000001 +#define LIBERO_SETTING_SGMII_PLL_PHADJ 0x00007443 +#define LIBERO_SETTING_SGMII_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_SGMII_MODE 0x08c0e6ff + +#define LIBERO_SETTING_RECAL_CNTL 0x000020c8 +#define LIBERO_SETTING_CLK_CNTL 0xf00050cc +#define LIBERO_SETTING_PLL_CNTL 0x80140101 + +#define LIBERO_SETTING_TRAINING_SKIP_SETTING 0x00000002 + +#define LIBERO_SETTING_DDRPHY_MODE_OFF 0x00000000 +#define LIBERO_SETTING_DPC_BITS_OFF_MODE 0x00000000 +#define LIBERO_SETTING_DDRPHY_MODE 0x00014a24 +#define LIBERO_SETTING_RPC_ODT_DQ 0x00000006 +#define LIBERO_SETTING_RPC_ODT_DQS 0x00000006 +#define LIBERO_SETTING_RPC_ODT_ADDCMD 0x00000002 +#define LIBERO_SETTING_RPC_ODT_CLK 0x00000002 +#define LIBERO_SETTING_RPC_EN_ADDCMD0_OVRT9 0x00000f00 +#define LIBERO_SETTING_RPC_EN_ADDCMD1_OVRT10 0x00000fff +#define LIBERO_SETTING_RPC_EN_ADDCMD2_OVRT11 0x00000fe6 +#define LIBERO_SETTING_RPC_EN_DATA0_OVRT12 0x00000000 +#define LIBERO_SETTING_RPC_EN_DATA1_OVRT13 0x00000000 +#define LIBERO_SETTING_RPC_EN_DATA2_OVRT14 0x00000000 +#define LIBERO_SETTING_RPC_EN_DATA3_OVRT15 0x00000000 +#define LIBERO_SETTING_RPC_EN_ECC_OVRT16 0x0000007f +#define LIBERO_SETTING_RPC235_WPD_ADD_CMD0 0x00000000 +#define LIBERO_SETTING_RPC236_WPD_ADD_CMD1 0x00000000 +#define LIBERO_SETTING_RPC237_WPD_ADD_CMD2 0x00000120 +#define LIBERO_SETTING_RPC238_WPD_DATA0 0x00000000 +#define LIBERO_SETTING_RPC239_WPD_DATA1 0x00000000 +#define LIBERO_SETTING_RPC240_WPD_DATA2 0x00000000 +#define LIBERO_SETTING_RPC241_WPD_DATA3 0x00000000 +#define LIBERO_SETTING_RPC242_WPD_ECC 0x00000000 +#define LIBERO_SETTING_RPC243_WPU_ADD_CMD0 0x00000fff +#define LIBERO_SETTING_RPC244_WPU_ADD_CMD1 0x00000fff +#define LIBERO_SETTING_RPC245_WPU_ADD_CMD2 0x00000edf +#define LIBERO_SETTING_RPC246_WPU_DATA0 0x000007ff +#define LIBERO_SETTING_RPC247_WPU_DATA1 0x000007ff +#define LIBERO_SETTING_RPC248_WPU_DATA2 0x000007ff +#define LIBERO_SETTING_RPC249_WPU_DATA3 0x000007ff +#define LIBERO_SETTING_RPC250_WPU_ECC 0x0000007f +#define LIBERO_SETTING_RPC_EN_ADDCMD1_OVRT10 0x00000fff +#define LIBERO_SETTING_RPC_EN_ADDCMD2_OVRT11 0x00000fe6 + +#define LIBERO_SETTING_DDR_SOFT_RESET 0x00000000 +#define LIBERO_SETTING_DDR_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_DDR_PLL_REF_FB 0x00000500 +#define LIBERO_SETTING_DDR_PLL_FRACN 0x00000000 +#define LIBERO_SETTING_DDR_PLL_DIV_0_1 0x02000100 +#define LIBERO_SETTING_DDR_PLL_DIV_2_3 0x01000100 +#define LIBERO_SETTING_DDR_PLL_CTRL2 0x00001020 +#define LIBERO_SETTING_DDR_PLL_CAL 0x00000d06 +#define LIBERO_SETTING_DDR_PLL_PHADJ 0x00005003 +#define LIBERO_SETTING_DDR_SSCG_REG_0 0x00000000 +#define LIBERO_SETTING_DDR_SSCG_REG_1 0x00000000 +#define LIBERO_SETTING_DDR_SSCG_REG_2 0x00000080 +#define LIBERO_SETTING_DDR_SSCG_REG_3 0x00000001 + +#define LIBERO_SETTING_CFG_MANUAL_ADDRESS_MAP 0x00000000 +#define LIBERO_SETTING_CFG_CHIPADDR_MAP 0x0000001d +#define LIBERO_SETTING_CFG_CIDADDR_MAP 0x00000000 +#define LIBERO_SETTING_CFG_BANKADDR_MAP_0 0x0000c2ca +#define LIBERO_SETTING_CFG_BANKADDR_MAP_1 0x00000000 +#define LIBERO_SETTING_CFG_ROWADDR_MAP_0 0x9140f38d +#define LIBERO_SETTING_CFG_ROWADDR_MAP_1 0x75955134 +#define LIBERO_SETTING_CFG_ROWADDR_MAP_2 0x71b69961 +#define LIBERO_SETTING_CFG_ROWADDR_MAP_3 0x00000000 +#define LIBERO_SETTING_CFG_COLADDR_MAP_0 0x440c2040 +#define LIBERO_SETTING_CFG_COLADDR_MAP_1 0x02481c61 +#define LIBERO_SETTING_CFG_COLADDR_MAP_2 0x00000000 +#define LIBERO_SETTING_CFG_VRCG_ENABLE 0x00000140 +#define LIBERO_SETTING_CFG_VRCG_DISABLE 0x000000a0 +#define LIBERO_SETTING_CFG_WRITE_LATENCY_SET 0x00000000 + +#define LIBERO_SETTING_CFG_THERMAL_OFFSET 0x00000000 +#define LIBERO_SETTING_CFG_SOC_ODT 0x00000006 +#define LIBERO_SETTING_CFG_ODTE_CK 0x00000000 +#define LIBERO_SETTING_CFG_ODTE_CS 0x00000000 +#define LIBERO_SETTING_CFG_ODTD_CA 0x00000000 +#define LIBERO_SETTING_CFG_LPDDR4_FSP_OP 0x00000001 +#define LIBERO_SETTING_CFG_DBI_CL 0x00000016 +#define LIBERO_SETTING_CFG_NON_DBI_CL 0x00000016 +#define LIBERO_SETTING_CFG_WRITE_CRC 0x00000000 +#define LIBERO_SETTING_CFG_MPR_READ_FORMAT 0x00000000 +#define LIBERO_SETTING_CFG_WR_CMD_LAT_CRC_DM 0x00000000 +#define LIBERO_SETTING_CFG_FINE_GRAN_REF_MODE 0x00000000 +#define LIBERO_SETTING_CFG_TEMP_SENSOR_READOUT 0x00000000 +#define LIBERO_SETTING_CFG_PER_DRAM_ADDR_EN 0x00000000 +#define LIBERO_SETTING_CFG_GEARDOWN_MODE 0x00000000 +#define LIBERO_SETTING_CFG_WR_PREAMBLE 0x00000001 +#define LIBERO_SETTING_CFG_RD_PREAMBLE 0x00000000 +#define LIBERO_SETTING_CFG_RD_PREAMB_TRN_MODE 0x00000000 +#define LIBERO_SETTING_CFG_SR_ABORT 0x00000000 + +#define LIBERO_SETTING_CFG_INT_VREF_MON 0x00000000 +#define LIBERO_SETTING_CFG_MAX_PWR_DOWN_MODE 0x00000000 +#define LIBERO_SETTING_CFG_READ_DBI 0x00000000 +#define LIBERO_SETTING_CFG_WRITE_DBI 0x00000000 +#define LIBERO_SETTING_CFG_DATA_MASK 0x00000001 +#define LIBERO_SETTING_CFG_RTT_PARK 0x00000000 +#define LIBERO_SETTING_CFG_ODT_INBUF_4_PD 0x00000000 +#define LIBERO_SETTING_CFG_CCD_S 0x00000005 +#define LIBERO_SETTING_CFG_CCD_L 0x00000006 +#define LIBERO_SETTING_CFG_VREFDQ_TRN_ENABLE 0x00000000 +#define LIBERO_SETTING_CFG_VREFDQ_TRN_RANGE 0x00000000 +#define LIBERO_SETTING_CFG_VREFDQ_TRN_VALUE 0x00000000 +#define LIBERO_SETTING_CFG_RRD_S 0x00000004 +#define LIBERO_SETTING_CFG_RRD_L 0x00000003 +#define LIBERO_SETTING_CFG_WTR_S 0x00000003 +#define LIBERO_SETTING_CFG_WTR_L 0x00000003 +#define LIBERO_SETTING_CFG_WTR_S_CRC_DM 0x00000003 +#define LIBERO_SETTING_CFG_WTR_L_CRC_DM 0x00000003 +#define LIBERO_SETTING_CFG_WR_CRC_DM 0x00000006 +#define LIBERO_SETTING_CFG_RFC1 0x00000036 +#define LIBERO_SETTING_CFG_RFC2 0x00000036 +#define LIBERO_SETTING_CFG_RFC4 0x00000036 +#define LIBERO_SETTING_CFG_NIBBLE_DEVICES 0x00000000 + +#define LIBERO_SETTING_CFG_MB_AUTOPCH_COL_BIT_POS_LOW 0x00000004 +#define LIBERO_SETTING_CFG_MB_AUTOPCH_COL_BIT_POS_HIGH 0x0000000a +#define LIBERO_SETTING_CFG_CA_PARITY_ERR_STATUS 0x00000000 +#define LIBERO_SETTING_CFG_CRC_ERROR_CLEAR 0x00000000 +#define LIBERO_SETTING_CFG_CA_PARITY_LATENCY 0x00000000 +#define LIBERO_SETTING_CFG_CA_PARITY_PERSIST_ERR 0x00000000 +#define LIBERO_SETTING_CFG_GENERATE_REFRESH_ON_SRX 0x00000001 +#define LIBERO_SETTING_CFG_CS_TO_CMDADDR_LATENCY 0x00000000 + +#define LIBERO_SETTING_CFG_TEMP_CTRL_REF_MODE 0x00000000 +#define LIBERO_SETTING_CFG_TEMP_CTRL_REF_RANGE 0x00000000 + +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS0_0 0x81881881 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS0_1 0x00008818 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS1_0 0xa92a92a9 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS1_1 0x00002a92 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS2_0 0xc28c28c2 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS2_1 0x00008c28 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS3_0 0xea2ea2ea +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS3_1 0x00002ea2 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS4_0 0x03903903 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS4_1 0x00009039 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS5_0 0x2b32b32b +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS5_1 0x000032b3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS6_0 0x44944944 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS6_1 0x00009449 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS7_0 0x6c36c36c +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS7_1 0x000036c3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS8_0 0x85985985 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS8_1 0x00009859 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS9_0 0xad3ad3ad +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS9_1 0x00003ad3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS10_0 0xc69c69c6 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS10_1 0x00009c69 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS11_0 0xee3ee3ee +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS11_1 0x00003ee3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS12_0 0x07a07a07 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS12_1 0x0000a07a +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS13_0 0x2f42f42f +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS13_1 0x000042f4 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS14_0 0x48a48a48 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS14_1 0x0000a48a +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS15_0 0x70470470 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS15_1 0x00004704 + +#define LIBERO_SETTING_CFG_NUM_LOGICAL_RANKS_PER_3DS 0x00000000 +#define LIBERO_SETTING_CFG_ADVANCE_ACTIVATE_READY 0x00000000 + +#define LIBERO_SETTING_CFG_RFC_DLR1 0x00000048 +#define LIBERO_SETTING_CFG_RFC_DLR2 0x0000002c +#define LIBERO_SETTING_CFG_RFC_DLR4 0x00000020 +#define LIBERO_SETTING_CFG_RRD_DLR 0x00000004 +#define LIBERO_SETTING_CFG_FAW_DLR 0x00000010 +#define LIBERO_SETTING_CTRLR_SOFT_RESET_N 0x00000001 +#define LIBERO_SETTING_CFG_LOOKAHEAD_PCH 0x00000000 +#define LIBERO_SETTING_CFG_LOOKAHEAD_ACT 0x00000000 +#define LIBERO_SETTING_CFG_BL 0x00000000 +#define LIBERO_SETTING_CTRLR_INIT 0x00000000 +#define LIBERO_SETTING_CFG_AUTO_REF_EN 0x00000001 +#define LIBERO_SETTING_CFG_RAS 0x00000022 +#define LIBERO_SETTING_CFG_RCD 0x0000000f +#define LIBERO_SETTING_CFG_RRD 0x00000008 +#define LIBERO_SETTING_CFG_RP 0x00000011 +#define LIBERO_SETTING_CFG_RC 0x00000033 +#define LIBERO_SETTING_CFG_FAW 0x00000020 +#define LIBERO_SETTING_CFG_RFC 0x00000130 +#define LIBERO_SETTING_CFG_RTP 0x00000008 +#define LIBERO_SETTING_CFG_WR 0x00000010 +#define LIBERO_SETTING_CFG_WTR 0x00000008 +#define LIBERO_SETTING_CFG_PASR 0x00000000 +#define LIBERO_SETTING_CFG_XP 0x00000006 +#define LIBERO_SETTING_CFG_XSR 0x0000001f +#define LIBERO_SETTING_CFG_CL 0x00000005 +#define LIBERO_SETTING_CFG_READ_TO_WRITE 0x0000000f +#define LIBERO_SETTING_CFG_WRITE_TO_WRITE 0x0000000f +#define LIBERO_SETTING_CFG_READ_TO_READ 0x0000000f +#define LIBERO_SETTING_CFG_WRITE_TO_READ 0x0000001f +#define LIBERO_SETTING_CFG_READ_TO_WRITE_ODT 0x00000001 +#define LIBERO_SETTING_CFG_WRITE_TO_WRITE_ODT 0x00000000 +#define LIBERO_SETTING_CFG_READ_TO_READ_ODT 0x00000001 +#define LIBERO_SETTING_CFG_WRITE_TO_READ_ODT 0x00000001 +#define LIBERO_SETTING_CFG_MIN_READ_IDLE 0x00000007 +#define LIBERO_SETTING_CFG_MRD 0x0000000c +#define LIBERO_SETTING_CFG_BT 0x00000000 +#define LIBERO_SETTING_CFG_DS 0x00000006 +#define LIBERO_SETTING_CFG_QOFF 0x00000000 +#define LIBERO_SETTING_CFG_RTT 0x00000002 +#define LIBERO_SETTING_CFG_DLL_DISABLE 0x00000000 +#define LIBERO_SETTING_CFG_REF_PER 0x00000c34 +#define LIBERO_SETTING_CFG_STARTUP_DELAY 0x00027100 +#define LIBERO_SETTING_CFG_MEM_COLBITS 0x0000000a +#define LIBERO_SETTING_CFG_MEM_ROWBITS 0x00000010 +#define LIBERO_SETTING_CFG_MEM_BANKBITS 0x00000003 + +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS0 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS1 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS2 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS3 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS4 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS5 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS6 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS7 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS0 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS1 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS2 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS3 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS4 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS5 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS6 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS7 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_TURN_ON 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_TURN_ON 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_TURN_OFF 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_TURN_OFF 0x00000000 + +#define LIBERO_SETTING_CFG_EMR3 0x00000000 +#define LIBERO_SETTING_CFG_TWO_T 0x00000000 +#define LIBERO_SETTING_CFG_TWO_T_SEL_CYCLE 0x00000001 +#define LIBERO_SETTING_CFG_REGDIMM 0x00000000 +#define LIBERO_SETTING_CFG_MOD 0x0000000c +#define LIBERO_SETTING_CFG_XS 0x00000005 +#define LIBERO_SETTING_CFG_XSDLL 0x00000200 +#define LIBERO_SETTING_CFG_XPR 0x00000005 +#define LIBERO_SETTING_CFG_AL_MODE 0x00000000 +#define LIBERO_SETTING_CFG_CWL 0x00000005 +#define LIBERO_SETTING_CFG_BL_MODE 0x00000000 +#define LIBERO_SETTING_CFG_TDQS 0x00000000 +#define LIBERO_SETTING_CFG_RTT_WR 0x00000000 +#define LIBERO_SETTING_CFG_LP_ASR 0x00000000 +#define LIBERO_SETTING_CFG_AUTO_SR 0x00000000 +#define LIBERO_SETTING_CFG_SRT 0x00000000 +#define LIBERO_SETTING_CFG_ADDR_MIRROR 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_TYPE 0x00000001 +#define LIBERO_SETTING_CFG_ZQ_CAL_PER 0x00027100 +#define LIBERO_SETTING_CFG_AUTO_ZQ_CAL_EN 0x00000000 +#define LIBERO_SETTING_CFG_MEMORY_TYPE 0x00000400 +#define LIBERO_SETTING_CFG_ONLY_SRANK_CMDS 0x00000000 +#define LIBERO_SETTING_CFG_NUM_RANKS 0x00000001 +#define LIBERO_SETTING_CFG_QUAD_RANK 0x00000000 +#define LIBERO_SETTING_CFG_EARLY_RANK_TO_WR_START 0x00000000 +#define LIBERO_SETTING_CFG_EARLY_RANK_TO_RD_START 0x00000000 +#define LIBERO_SETTING_CFG_PASR_BANK 0x00000000 +#define LIBERO_SETTING_CFG_PASR_SEG 0x00000000 +#define LIBERO_SETTING_CFG_INIT_DURATION 0x00000640 +#define LIBERO_SETTING_CFG_ZQINIT_CAL_DURATION 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_L_DURATION 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_S_DURATION 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_R_DURATION 0x00000028 +#define LIBERO_SETTING_CFG_MRR 0x00000008 +#define LIBERO_SETTING_CFG_MRW 0x0000000a +#define LIBERO_SETTING_CFG_ODT_POWERDOWN 0x00000000 +#define LIBERO_SETTING_CFG_WL 0x00000008 +#define LIBERO_SETTING_CFG_RL 0x0000000e +#define LIBERO_SETTING_CFG_CAL_READ_PERIOD 0x00000000 +#define LIBERO_SETTING_CFG_NUM_CAL_READS 0x00000001 + +#define LIBERO_SETTING_INIT_FORCE_WRITE_DATA_0 0x00000000 +#define LIBERO_SETTING_INIT_AUTOINIT_DISABLE 0x00000000 +#define LIBERO_SETTING_INIT_FORCE_RESET 0x00000000 +#define LIBERO_SETTING_INIT_GEARDOWN_EN 0x00000000 +#define LIBERO_SETTING_INIT_DISABLE_CKE 0x00000000 +#define LIBERO_SETTING_INIT_CS 0x00000000 +#define LIBERO_SETTING_INIT_PRECHARGE_ALL 0x00000000 +#define LIBERO_SETTING_INIT_REFRESH 0x00000000 +#define LIBERO_SETTING_INIT_ZQ_CAL_REQ 0x00000000 +#define LIBERO_SETTING_INIT_MRR_MODE 0x00000000 +#define LIBERO_SETTING_INIT_MR_W_REQ 0x00000000 +#define LIBERO_SETTING_INIT_MR_ADDR 0x00000000 +#define LIBERO_SETTING_INIT_MR_WR_DATA 0x00000000 +#define LIBERO_SETTING_INIT_MR_WR_MASK 0x00000000 +#define LIBERO_SETTING_INIT_NOP 0x00000000 +#define LIBERO_SETTING_INIT_SELF_REFRESH 0x00000000 +#define LIBERO_SETTING_INIT_POWER_DOWN 0x00000000 +#define LIBERO_SETTING_INIT_FORCE_WRITE 0x00000000 +#define LIBERO_SETTING_INIT_FORCE_WRITE_CS 0x00000000 +#define LIBERO_SETTING_INIT_RDIMM_COMPLETE 0x00000000 +#define LIBERO_SETTING_INIT_MEMORY_RESET_MASK 0x00000000 +#define LIBERO_SETTING_INIT_CAL_SELECT 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_R_REQ 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_B_SIZE 0x00000000 +#define LIBERO_SETTING_INIT_RWFIFO 0x00000000 +#define LIBERO_SETTING_INIT_RD_DQCAL 0x00000000 +#define LIBERO_SETTING_INIT_START_DQSOSC 0x00000000 +#define LIBERO_SETTING_INIT_STOP_DQSOSC 0x00000000 +#define LIBERO_SETTING_INIT_ZQ_CAL_START 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_ADDR_0 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_ADDR_1 0x00000000 +#define LIBERO_SETTING_INIT_ODT_FORCE_EN 0x00000000 +#define LIBERO_SETTING_INIT_ODT_FORCE_RANK 0x00000000 +#define LIBERO_SETTING_INIT_PDA_MR_W_REQ 0x00000000 +#define LIBERO_SETTING_INIT_PDA_NIBBLE_SELECT 0x00000000 +#define LIBERO_SETTING_INIT_WRITE_DATA_1B_ECC_ERROR_GEN 0x00000000 +#define LIBERO_SETTING_INIT_WRITE_DATA_2B_ECC_ERROR_GEN 0x00000000 +#define LIBERO_SETTING_INIT_READ_CAPTURE_ADDR 0x00000000 +#define LIBERO_SETTING_INIT_CA_PARITY_ERROR_GEN_REQ 0x00000000 +#define LIBERO_SETTING_INIT_CA_PARITY_ERROR_GEN_CMD 0x00000000 +#define LIBERO_SETTING_INIT_DFI_LP_DATA_REQ 0x00000000 +#define LIBERO_SETTING_INIT_DFI_LP_CTRL_REQ 0x00000000 +#define LIBERO_SETTING_INIT_DFI_LP_WAKEUP 0x00000000 +#define LIBERO_SETTING_INIT_DFI_DRAM_CLK_DISABLE 0x00000000 + +#define LIBERO_SETTING_CFG_CTRLR_INIT_DISABLE 0x00000000 +#define LIBERO_SETTING_CFG_RDIMM_LAT 0x00000000 +#define LIBERO_SETTING_CFG_RDIMM_BSIDE_INVERT 0x00000001 +#define LIBERO_SETTING_CFG_LRDIMM 0x00000000 +#define LIBERO_SETTING_CFG_RD_PREAMB_TOGGLE 0x00000000 +#define LIBERO_SETTING_CFG_RD_POSTAMBLE 0x00000000 +#define LIBERO_SETTING_CFG_PU_CAL 0x00000001 +#define LIBERO_SETTING_CFG_DQ_ODT 0x00000002 +#define LIBERO_SETTING_CFG_CA_ODT 0x00000004 +#define LIBERO_SETTING_CFG_ZQLATCH_DURATION 0x00000018 + +#define LIBERO_SETTING_CFG_WR_POSTAMBLE 0x00000000 +#define LIBERO_SETTING_CFG_CTRLUPD_TRIG 0x00000000 +#define LIBERO_SETTING_CFG_CTRLUPD_START_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_CTRLUPD_MAX 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_SEL 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_VALUE 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_TURN_OFF_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_SLOW_RESTART_WIN 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_RESTART_HOLDOFF 0x00000000 +#define LIBERO_SETTING_CFG_PARITY_RDIMM_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_ENABLE 0x00000000 +#define LIBERO_SETTING_CFG_ASYNC_ODT 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_DURATION 0x00000320 +#define LIBERO_SETTING_CFG_MRRI 0x00000012 +#define LIBERO_SETTING_CFG_PHYUPD_ACK_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_MIRROR_X16_BG0_BG1 0x00000000 +#define LIBERO_SETTING_CFG_DRAM_CLK_DISABLE_IN_SELF_RFH 0x00000000 +#define LIBERO_SETTING_CFG_CKSRE 0x00000008 +#define LIBERO_SETTING_CFG_CKSRX 0x0000000b +#define LIBERO_SETTING_CFG_RCD_STAB 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_CTRL_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_DRAM_CLK_ENABLE 0x00000000 +#define LIBERO_SETTING_CFG_IDLE_TIME_TO_SELF_REFRESH 0x00000000 +#define LIBERO_SETTING_CFG_IDLE_TIME_TO_POWER_DOWN 0x00000000 +#define LIBERO_SETTING_CFG_BURST_RW_REFRESH_HOLDOFF 0x00000000 +#define LIBERO_SETTING_CFG_BG_INTERLEAVE 0x00000001 +#define LIBERO_SETTING_CFG_REFRESH_DURING_PHY_TRAINING 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P0 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P1 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P2 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P3 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P4 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P5 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P6 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P7 0x00000000 +#define LIBERO_SETTING_CFG_REORDER_EN 0x00000001 +#define LIBERO_SETTING_CFG_REORDER_QUEUE_EN 0x00000001 +#define LIBERO_SETTING_CFG_INTRAPORT_REORDER_EN 0x00000000 +#define LIBERO_SETTING_CFG_MAINTAIN_COHERENCY 0x00000001 +#define LIBERO_SETTING_CFG_Q_AGE_LIMIT 0x000000ff +#define LIBERO_SETTING_CFG_RO_CLOSED_PAGE_POLICY 0x00000000 +#define LIBERO_SETTING_CFG_REORDER_RW_ONLY 0x00000000 +#define LIBERO_SETTING_CFG_RO_PRIORITY_EN 0x00000000 +#define LIBERO_SETTING_CFG_DM_EN 0x00000001 +#define LIBERO_SETTING_CFG_RMW_EN 0x00000000 +#define LIBERO_SETTING_CFG_ECC_CORRECTION_EN 0x00000000 +#define LIBERO_SETTING_CFG_ECC_BYPASS 0x00000000 +#define LIBERO_SETTING_CFG_ECC_1BIT_INT_THRESH 0x00000000 +#define LIBERO_SETTING_CFG_ERROR_GROUP_SEL 0x00000000 +#define LIBERO_SETTING_CFG_DATA_SEL 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MODE 0x00000000 +#define LIBERO_SETTING_CFG_POST_TRIG_CYCS 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MASK 0x00000000 +#define LIBERO_SETTING_CFG_EN_MASK 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MT_ADDR_0 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MT_ADDR_1 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_0 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_1 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_2 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_3 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_4 0x00000000 + +#define LIBERO_SETTING_MTC_ACQ_ADDR 0x00000000 +#define LIBERO_SETTING_MTC_ACQ_WR_DATA_0 0x00000000 +#define LIBERO_SETTING_MTC_ACQ_WR_DATA_1 0x00000000 +#define LIBERO_SETTING_MTC_ACQ_WR_DATA_2 0x00000000 + +#define LIBERO_SETTING_CFG_PRE_TRIG_CYCS 0x00000000 +#define LIBERO_SETTING_CFG_DATA_SEL_FIRST_ERROR 0x00000000 +#define LIBERO_SETTING_CFG_DQ_WIDTH 0x00000000 +#define LIBERO_SETTING_CFG_ACTIVE_DQ_SEL 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_RDDATA_EN 0x00000015 +#define LIBERO_SETTING_CFG_DFI_T_PHY_RDLAT 0x00000006 +#define LIBERO_SETTING_CFG_DFI_T_PHY_WRLAT 0x00000003 +#define LIBERO_SETTING_CFG_DFI_PHYUPD_EN 0x00000001 +#define LIBERO_SETTING_CFG_DFI_DATA_BYTE_DISABLE 0x00000000 +#define LIBERO_SETTING_CFG_DFI_LVL_SEL 0x00000000 +#define LIBERO_SETTING_CFG_DFI_LVL_PERIODIC 0x00000000 +#define LIBERO_SETTING_CFG_DFI_LVL_PATTERN 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI1_0 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI1_1 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI2_0 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI2_1 0x00000000 +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI1_0 0x7fffffff +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI1_1 0x00000000 +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI2_0 0x7fffffff +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI2_1 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI1_0 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI1_1 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI2_0 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI2_1 0x00000000 +#define LIBERO_SETTING_CFG_ENABLE_BUS_HOLD_AXI1 0x00000000 +#define LIBERO_SETTING_CFG_ENABLE_BUS_HOLD_AXI2 0x00000000 +#define LIBERO_SETTING_CFG_AXI_AUTO_PCH 0x00000000 + +#define LIBERO_SETTING_PHY_DFI_INIT_START 0x00000001 +#define LIBERO_SETTING_PHY_RESET_CONTROL 0x00008001 +#define LIBERO_SETTING_PHY_PC_RANK 0x00000001 +#define LIBERO_SETTING_PHY_RANKS_TO_TRAIN 0x00000001 +#define LIBERO_SETTING_PHY_WRITE_REQUEST 0x00000000 +#define LIBERO_SETTING_PHY_READ_REQUEST 0x00000000 +#define LIBERO_SETTING_PHY_WRITE_LEVEL_DELAY 0x00000000 +#define LIBERO_SETTING_PHY_GATE_TRAIN_DELAY 0x0000003f +#define LIBERO_SETTING_PHY_EYE_TRAIN_DELAY 0x0000003f +#define LIBERO_SETTING_PHY_EYE_PAT 0x00000000 +#define LIBERO_SETTING_PHY_START_RECAL 0x00000000 +#define LIBERO_SETTING_PHY_CLR_DFI_LVL_PERIODIC 0x00000000 +#define LIBERO_SETTING_PHY_TRAIN_STEP_ENABLE 0x00000018 +#define LIBERO_SETTING_PHY_LPDDR_DQ_CAL_PAT 0x00000000 +#define LIBERO_SETTING_PHY_INDPNDT_TRAINING 0x00000001 +#define LIBERO_SETTING_PHY_ENCODED_QUAD_CS 0x00000000 +#define LIBERO_SETTING_PHY_HALF_CLK_DLY_ENABLE 0x00000000 + +#define LIBERO_SETTING_SEG0_0 0x80007f80 +#define LIBERO_SETTING_SEG0_1 0x80007030 +#define LIBERO_SETTING_SEG0_2 0x00000000 +#define LIBERO_SETTING_SEG0_3 0x00000000 +#define LIBERO_SETTING_SEG0_4 0x00000000 +#define LIBERO_SETTING_SEG0_5 0x00000000 +#define LIBERO_SETTING_SEG0_6 0x00000000 +#define LIBERO_SETTING_SEG0_7 0x00000000 +#define LIBERO_SETTING_SEG1_0 0x00000000 +#define LIBERO_SETTING_SEG1_1 0x00000000 +#define LIBERO_SETTING_SEG1_2 0x80007fb0 +#define LIBERO_SETTING_SEG1_3 0x80000000 +#define LIBERO_SETTING_SEG1_4 0x80007fa0 +#define LIBERO_SETTING_SEG1_5 0x80000000 +#define LIBERO_SETTING_SEG1_6 0x00000000 +#define LIBERO_SETTING_SEG1_7 0x00000000 + +#define LIBERO_SETTING_TIP_CONFIG_PARAMS_BCLK_VCOPHS_OFFSET 0x00000002 +#define LIBERO_SETTING_DDR_CLK 1600000000 + +#define LIBERO_SETTING_REFCLK_DDR3_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_DDR4_1600_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_NUM_OFFSETS 4 +#define LIBERO_SETTING_REFCLK_DDR3_1067_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_DDR4_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_3 1 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_0 7 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_1 0 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_2 7 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_0 7 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_1 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_2 1 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_1 5 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_2 1 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_3 5 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_1 2 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_3 2 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_1 2 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_3 2 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_2 7 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_2 6 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_1 2 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_2 3 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_3 0 + +#define LIBERO_SETTING_TIP_CFG_PARAMS 0x07cfe02f + +#define LIBERO_SETTING_DDR_32_CACHE 0x80000000 +#define LIBERO_SETTING_DDR_32_CACHE_SIZE 0x100000 +#define LIBERO_SETTING_DDR_64_CACHE 0x1000000000 +#define LIBERO_SETTING_DDR_64_CACHE_SIZE 0x100000 +#define LIBERO_SETTING_DDR_32_NON_CACHE 0xc0000000 +#define LIBERO_SETTING_DDR_32_NON_CACHE_SIZE 0x100000 +#define LIBERO_SETTING_DDR_64_NON_CACHE 0x1400000000 +#define LIBERO_SETTING_DDR_64_NON_CACHE_SIZE 0x100000 + +#define LIBERO_SETTING_DPC_BITS 0x00050422 +#define LIBERO_SETTING_DATA_LANES_USED 0x00000004 + +/* Cache settings */ + +#define LIBERO_SETTING_WAY_MASK_DMA 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_0 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_1 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_2 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_3 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_E51_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_E51_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_1_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_1_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_2_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_2_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_3_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_3_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_4_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_4_ICACHE 0x0000f0ff +#define LIBERO_SETTING_NUM_SCRATCH_PAD_WAYS 0x00000004 +#define LIBERO_SETTING_L2_SHUTDOWN_CR 0x00000000 +#define LIBERO_SETTING_WAY_ENABLE 0x0000000b + +#endif /* __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_LIBERODEFS_H */ diff --git a/boards/ssrc/icicle/nuttx-config/include/board_memorymap.h b/boards/ssrc/icicle/nuttx-config/include/board_memorymap.h new file mode 100644 index 000000000000..0f0f92e3a9de --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/include/board_memorymap.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/include/board_memorymap.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_MEMORYMAP_H +#define __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_MEMORYMAP_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* DDR start address */ + +#define MPFS_DDR_BASE (0x80000000) +#define MPFS_DDR_SIZE (0x40000000) + +/* Kernel code memory (RX) */ + +#define KFLASH_START (uintptr_t)&__kflash_start +#define KFLASH_SIZE (uintptr_t)&__kflash_size +#define KSRAM_START (uintptr_t)&__ksram_start +#define KSRAM_SIZE (uintptr_t)&__ksram_size +#define KSRAM_END (uintptr_t)&__ksram_end + +/* Kernel RAM (RW) */ + +#define PGPOOL_START (uintptr_t)&__pgheap_start +#define PGPOOL_SIZE (uintptr_t)&__pgheap_size + +/* Page pool (RWX) */ + +#define PGPOOL_START (uintptr_t)&__pgheap_start +#define PGPOOL_SIZE (uintptr_t)&__pgheap_size +#define PGPOOL_END (PGPOOL_START + PGPOOL_SIZE) + +/* User flash */ + +#define UFLASH_START (uintptr_t)&__uflash_start +#define UFLASH_SIZE (uintptr_t)&__uflash_size + +/* User RAM */ + +#define USRAM_START (uintptr_t)&__usram_start +#define USRAM_SIZE (uintptr_t)&__usram_size + +/* User IO */ + +#define USRIO_START (uintptr_t)&__usrio_start +#define USRIO_SIZE (uintptr_t)&__usrio_size + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Kernel code memory (RX) */ + +extern uintptr_t __kflash_start; +extern uintptr_t __kflash_size; + +/* Kernel RAM (RW) */ + +extern uintptr_t __ksram_start; +extern uintptr_t __ksram_size; +extern uintptr_t __ksram_end; + +/* Page pool (RWX) */ + +extern uintptr_t __pgheap_start; +extern uintptr_t __pgheap_size; + +/* User code memory (RX) */ + +extern uintptr_t __uflash_start; +extern uintptr_t __uflash_size; + +/* User RAM (RW) */ + +extern uintptr_t __usram_start; +extern uintptr_t __usram_size; + +/* User IO (R) */ + +extern uintptr_t __usrio_start; +extern uintptr_t __usrio_size; + +#endif /* __BOARDS_RISC_V_MPFS_ICICLE_INCLUDE_BOARD_MEMORYMAP_H */ diff --git a/boards/ssrc/icicle/nuttx-config/minimal/defconfig b/boards/ssrc/icicle/nuttx-config/minimal/defconfig new file mode 100644 index 000000000000..9c6b3ae60ddd --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/minimal/defconfig @@ -0,0 +1,150 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_FS_ROMFS_CACHE_NODE is not set +# CONFIG_MPFS_MAC_AUTONEG is not set +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ssrc/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=54000 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_EPBULKIN=3 +CONFIG_CDCACM_EPINTIN=4 +CONFIG_CDCACM_PRODUCTID=0x05e1 +CONFIG_CDCACM_PRODUCTSTR="SSRC Icicle v1.0" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x16c0 +CONFIG_CDCACM_VENDORSTR="SSRC" +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_ZERO=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_FAT_DIRECT_RETRY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_EXCLUDE_ENVIRON=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_DRIVER=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=8192 +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIBM=y +CONFIG_M25P_SUBSECTOR_ERASE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MPFS_COREPWM0=y +CONFIG_MPFS_COREPWM0_PWMCLK=62500000 +CONFIG_MPFS_COREPWM1=y +CONFIG_MPFS_COREPWM1_NCHANNELS=1 +CONFIG_MPFS_COREPWM1_PWMCLK=62500000 +CONFIG_MPFS_EMMCSD=y +CONFIG_MPFS_ENABLE_DPFPU=y +CONFIG_MPFS_HAVE_COREPWM=y +CONFIG_MPFS_I2C0=y +CONFIG_MPFS_I2C1=y +CONFIG_MPFS_SPI0=y +CONFIG_MPFS_SPI1=y +CONFIG_MPFS_UART0=y +CONFIG_MPFS_UART1=y +CONFIG_MPFS_UART2=y +CONFIG_MPFS_UART4=y +CONFIG_MTD=y +CONFIG_MTD_M25P=y +CONFIG_MTD_PARTITION=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MKDIR=y +CONFIG_NSH_DISABLE_PRINTF=y +CONFIG_NSH_DISABLE_RM=y +CONFIG_NSH_DISABLE_RMDIR=y +CONFIG_NSH_DISABLE_TRUNCATE=y +CONFIG_NSH_DISABLE_UMOUNT=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_PROMPT_STRING="icicle-min> " +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTABLE_PARTITION=y +CONFIG_PTHREAD_STACK_MIN=1024 +CONFIG_PWM=y +CONFIG_PWM_NCHANNELS=8 +CONFIG_RAM_SIZE=2097152 +CONFIG_RAM_START=0xAD000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=4096 +CONFIG_SCHED_WAITPID=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI_DRIVER=y +CONFIG_STACK_COLORATION=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_NSH_STACKSIZE=2304 +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/ssrc/icicle/nuttx-config/nsh/defconfig b/boards/ssrc/icicle/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..9193fbba3340 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/nsh/defconfig @@ -0,0 +1,181 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_FS_ROMFS_CACHE_NODE is not set +# CONFIG_MPFS_MAC_AUTONEG is not set +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ssrc/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=54000 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_EPBULKIN=3 +CONFIG_CDCACM_EPINTIN=4 +CONFIG_CDCACM_PRODUCTID=0x05e1 +CONFIG_CDCACM_PRODUCTSTR="SSRC Icicle v1.0" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x16c0 +CONFIG_CDCACM_VENDORSTR="SSRC" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_ZERO=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EE24XX_FREQUENCY=400000 +CONFIG_EEPROM=y +CONFIG_FAT_DIRECT_RETRY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_EXCLUDE_ENVIRON=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_DRIVER=y +CONFIG_I2C_EE_24XX=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=8192 +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIBM=y +CONFIG_M25P_SUBSECTOR_ERASE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MPFS_COREPWM0=y +CONFIG_MPFS_COREPWM0_PWMCLK=62500000 +CONFIG_MPFS_COREPWM1=y +CONFIG_MPFS_COREPWM1_NCHANNELS=1 +CONFIG_MPFS_COREPWM1_PWMCLK=62500000 +CONFIG_MPFS_EMMCSD=y +CONFIG_MPFS_ENABLE_DPFPU=y +CONFIG_MPFS_ETHMAC_0=y +CONFIG_MPFS_HAVE_COREPWM=y +CONFIG_MPFS_I2C0=y +CONFIG_MPFS_I2C1=y +CONFIG_MPFS_MAC_ETHFD=y +CONFIG_MPFS_PHYADDR=9 +CONFIG_MPFS_PHYINIT=y +CONFIG_MPFS_SPI0=y +CONFIG_MPFS_SPI1=y +CONFIG_MPFS_UART0=y +CONFIG_MPFS_UART1=y +CONFIG_MPFS_UART2=y +CONFIG_MPFS_UART3=y +CONFIG_MPFS_UART4=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_M25P=y +CONFIG_MTD_PARTITION=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DRIPADDR=0xc0a8c864 +CONFIG_NETINIT_IPADDR=0xc0a8c865 +CONFIG_NETINIT_MACADDR_1=0x554b4b41 +CONFIG_NETINIT_MACADDR_2=0x024a +CONFIG_NETINIT_NOMAC=y +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_TCP=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_NOTIFIER=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_NOTIFIER=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_PROMPT_STRING="icicle> " +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTABLE_PARTITION=y +CONFIG_PTHREAD_STACK_MIN=1024 +CONFIG_PWM=y +CONFIG_PWM_NCHANNELS=8 +CONFIG_RAM_SIZE=2097152 +CONFIG_RAM_START=0xAD000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=4096 +CONFIG_SCHED_WAITPID=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI_DRIVER=y +CONFIG_STACK_COLORATION=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_MDIO=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_NSH_STACKSIZE=2304 +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_UART1_RXBUFSIZE=600 +CONFIG_UART1_TXBUFSIZE=1500 +CONFIG_UART3_RXBUFSIZE=600 +CONFIG_UART3_TXBUFSIZE=3000 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/ssrc/icicle/nuttx-config/protected/defconfig b/boards/ssrc/icicle/nuttx-config/protected/defconfig new file mode 100644 index 000000000000..52e9ae5acec3 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/protected/defconfig @@ -0,0 +1,189 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_FS_ROMFS_CACHE_NODE is not set +# CONFIG_MPFS_MAC_AUTONEG is not set +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ssrc/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_USE_MMU=y +CONFIG_ARCH_USE_MPU=y +CONFIG_BOARDCTL_IOCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=54000 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILD_PROTECTED=y +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_EPBULKIN=3 +CONFIG_CDCACM_EPINTIN=4 +CONFIG_CDCACM_PRODUCTID=0x05e1 +CONFIG_CDCACM_PRODUCTSTR="SSRC Icicle v1.0" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x16c0 +CONFIG_CDCACM_VENDORSTR="SSRC" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_ZERO=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EE24XX_FREQUENCY=400000 +CONFIG_EEPROM=y +CONFIG_FAT_DIRECT_RETRY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_DRIVER=y +CONFIG_I2C_EE_24XX=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="px4_entry" +CONFIG_INIT_STACKSIZE=8192 +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_PERROR_STDOUT=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIBC_USRWORK=y +CONFIG_LIBC_USRWORKSTACKSIZE=4096 +CONFIG_LIBM=y +CONFIG_M25P_SUBSECTOR_ERASE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MPFS_COREPWM0=y +CONFIG_MPFS_COREPWM0_PWMCLK=62500000 +CONFIG_MPFS_COREPWM1=y +CONFIG_MPFS_COREPWM1_NCHANNELS=1 +CONFIG_MPFS_COREPWM1_PWMCLK=62500000 +CONFIG_MPFS_EMMCSD=y +CONFIG_MPFS_ENABLE_DPFPU=y +CONFIG_MPFS_ETHMAC_0=y +CONFIG_MPFS_HAVE_COREPWM=y +CONFIG_MPFS_I2C0=y +CONFIG_MPFS_I2C1=y +CONFIG_MPFS_MAC_ETHFD=y +CONFIG_MPFS_PHYADDR=9 +CONFIG_MPFS_PHYINIT=y +CONFIG_MPFS_SPI0=y +CONFIG_MPFS_SPI1=y +CONFIG_MPFS_UART0=y +CONFIG_MPFS_UART1=y +CONFIG_MPFS_UART2=y +CONFIG_MPFS_UART3=y +CONFIG_MPFS_UART4=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_M25P=y +CONFIG_MTD_PARTITION=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DRIPADDR=0xc0a8c864 +CONFIG_NETINIT_IPADDR=0xc0a8c865 +CONFIG_NETINIT_MACADDR_1=0x554b4b41 +CONFIG_NETINIT_MACADDR_2=0x024a +CONFIG_NETINIT_NOMAC=y +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_TCP=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_NOTIFIER=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_NOTIFIER=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_PROMPT_STRING="icicle-prot> " +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_NUTTX_USERSPACE=0xACE00000 +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTABLE_PARTITION=y +CONFIG_PTHREAD_STACK_MIN=1024 +CONFIG_PWM=y +CONFIG_PWM_NCHANNELS=8 +CONFIG_RAM_SIZE=2097152 +CONFIG_RAM_START=0xAD000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=4096 +CONFIG_SCHED_WAITPID=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI_DRIVER=y +CONFIG_STACK_COLORATION=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_MDIO=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_NSH_STACKSIZE=2304 +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_UART1_RXBUFSIZE=600 +CONFIG_UART1_TXBUFSIZE=1500 +CONFIG_UART3_RXBUFSIZE=600 +CONFIG_UART3_TXBUFSIZE=3000 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/ssrc/icicle/nuttx-config/scripts/bootloader_script.ld b/boards/ssrc/icicle/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..affa1c0bfbd3 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,168 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + + +/*----------------------------------------------------------------------------- + + -- PolarFire SoC Memorp map + + + 0x0800_0000 +-----------------------+ + | DTIM Len=8K | + +-----------------------+ + + 0x0180_0000 +-----------------------+ + | ITIM Hart 0 | + +-----------------------+ + eNVM detail + 0x0180_8000 +-----------------------+ +-------------------+ - + | ITIM Hart 1 Len=28K | |Sector 2 | ^ + +-----------------------+ |0x2022_0000 8K | | + +-------------------+ | + 0x0181_0000 +-----------------------+ |Sector 0 | 1 + | ITIM Hart 2 Len=28K | |0x2022_2000 56K | 2 + +-----------------------+ +-------------------+ 8 + |Sector 1 | k + 0x0181_8000 +-----------------------+ |0x2022_3000 56K | | + | ITIM Hart 3 Len=28K | +-------------------+ | + +-----------------------+ |Sector 3 | | + |0x2022_3E00 8K | v + 0x0182_0000 +-----------------------+ +-------------------+ - + | ITIM Hart 4 Len=28K | + +-----------------------+ + + 0x0800_0000 +-----------------------+ + | LIM Len max=1920K | +------>+------+ + +-----------------------+ | | | + | | | + 0x2022_0000 +-----------------------+ | | | + | eNVM 128K | | | | + +-----------------------+ | | | + | | | + 0x8000_0000 +-----------------------+--+ | DDR | + 0x10_0000_0000 | DDR cached | | | + | SEG0 | +--->| | + +-----------------------+ | | | + | | | + 0xC000_0000 +-----------------------+-----+ | | + 0x14_0000_0000 | DDR non-cached | | | + | SEG1 | | | + +-----------------------+ +-->+ | + | | | + 0xD000_0000 +-----------------------+------+ | | + 0x18_0000_0000 | Write Combine buffer | +------+ + | SEG1 | + +-----------------------+ + ----------------------------------------------------------------------------*/ + +MEMORY +{ + envm (rx) : ORIGIN = 0x20220100, LENGTH = 128K - 256 /* 256 reserved for hss headers */ + l2lim (rwx) : ORIGIN = 0x08000000, LENGTH = 1024k + l2zerodevice (rwx) : ORIGIN = 0x0A000000, LENGTH = 512k +} + +OUTPUT_ARCH("riscv") + +ENTRY(_stext) + +EXTERN(__start) + +SECTIONS +{ + .l2_scratchpad : ALIGN(0x10) + { + __l2_scratchpad_load = LOADADDR(.l2_scratchpad); + __l2_scratchpad_start = .; + __l2_scratchpad_vma_start = .; + *(.l2_scratchpad) + + . = ALIGN(0x10); + __l2_scratchpad_end = .; + __l2_scratchpad_vma_end = .; + } > l2zerodevice + + PROVIDE(__mpfs_nuttx_start = ORIGIN(l2lim)); + + .text : { + _stext = ABSOLUTE(.); + *(.start .start.*) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.* .srodata .srodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > envm + + .init_section : ALIGN(4) { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > envm + + _eronly = ABSOLUTE(.); + + .data : ALIGN(4) { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.sdata .sdata.* .sdata2.*) + *(.gnu.linkonce.d.*) + *(.gnu.linkonce.s.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > l2lim AT > envm + + PROVIDE(__global_pointer$ = _sdata + ((_edata - _sdata) / 2)); + + .bss : ALIGN(4) { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.sbss .sbss.*) + *(.gnu.linkonce.b.*) + *(.gnu.linkonce.sb.*) + *(COMMON) + . = ALIGN(32); + _ebss = ABSOLUTE(.); + } > l2lim + + PROVIDE(__mpfs_nuttx_end = .); + + /* 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/boards/ssrc/icicle/nuttx-config/scripts/kernel-space.ld b/boards/ssrc/icicle/nuttx-config/scripts/kernel-space.ld new file mode 100755 index 000000000000..b124a05314ed --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/scripts/kernel-space.ld @@ -0,0 +1,144 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* NOTE: This depends on the memory.ld script having been included prior to + * this script. + */ + +OUTPUT_ARCH("riscv") + +/* Provide these so there is no need for using config files for this */ + +__uflash_start = ORIGIN(uflash); +__uflash_size = LENGTH(uflash); +__usram_start = ORIGIN(usram); +__usram_size = LENGTH(usram); +__usrio_start = ORIGIN(usrio); +__usrio_size = LENGTH(usrio); + +/* Provide the kernel boundaries as well */ + +__kflash_start = ORIGIN(kflash); +__kflash_size = LENGTH(kflash); +__ksram_start = ORIGIN(ksram); +__ksram_size = LENGTH(ksram); +__ksram_end = ORIGIN(ksram) + LENGTH(ksram); + +ENTRY(__start) +EXTERN(__start) +EXTERN(_main_toc) +EXTERN(_main_toc_sig) + +SECTIONS +{ + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(8); + } > kflash + + .start : { + *(.start .start.*) + } > kflash + + .text : { + _stext = ABSOLUTE(.); + /* + This signature provides the bootloader with a way to delay booting + */ + . = 0x400; + _bootdelay_signature = ABSOLUTE(.); + FILL(0x01ecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.* .srodata .srodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > kflash + + .init_section : ALIGN(4) { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > kflash + + _eronly = ABSOLUTE(.); + + .data : ALIGN(4) { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.sdata .sdata.* .sdata2.*) + *(.gnu.linkonce.d.*) + *(.gnu.linkonce.s.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > ksram AT > kflash + + PROVIDE(__global_pointer$ = _sdata + ((_edata - _sdata) / 2)); + + .bss : ALIGN(4) { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.sbss .sbss.*) + *(.gnu.linkonce.b.*) + *(.gnu.linkonce.sb.*) + *(COMMON) + } > ksram + + /* Page tables here, align to 4K boundary */ + .pgtables : ALIGN(0x1000) { + *(.pgtables) + . = ALIGN(32); + _ebss = ABSOLUTE(.); + } > ksram + + /* 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/boards/ssrc/icicle/nuttx-config/scripts/memory.ld b/boards/ssrc/icicle/nuttx-config/scripts/memory.ld new file mode 100755 index 000000000000..c115778b0645 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/scripts/memory.ld @@ -0,0 +1,29 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/scripts/memory.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + kflash (rx) : ORIGIN = 0xACA00000, LENGTH = 4M /* w/ cache */ + ksram (rx) : ORIGIN = 0xAD000000, LENGTH = 2M /* w/ cache */ + + uflash (rwx) : ORIGIN = 0xACE00000, LENGTH = 2M /* w/ cache */ + usram (rwx) : ORIGIN = 0xAD200000, LENGTH = 2M /* w/ cache */ + usrio (r) : ORIGIN = 0xAD3F8000, LENGTH = 32K +} diff --git a/boards/ssrc/icicle/nuttx-config/scripts/script.ld b/boards/ssrc/icicle/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..f6dbf66c3ae6 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/scripts/script.ld @@ -0,0 +1,125 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + progmem (rx) : ORIGIN = 0xACA00000, LENGTH = 6M /* w/ cache */ + sram (rwx) : ORIGIN = 0xAD000000, LENGTH = 4M /* w/ cache */ +} + +OUTPUT_ARCH("riscv") + +ENTRY(__start) +EXTERN(__start) +EXTERN(_main_toc) +EXTERN(_main_toc_sig) +EXTERN(board_get_manifest) + +SECTIONS +{ + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + .toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(8); + } > progmem + + .start : { + *(.start .start.*) + } > progmem + + .text : { + _stext = ABSOLUTE(.); + /* + This signature provides the bootloader with a way to delay booting + */ + . = 0x400; + _bootdelay_signature = ABSOLUTE(.); + FILL(0x01ecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.* .srodata .srodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > progmem + + .init_section : ALIGN(4) { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > progmem + + _eronly = ABSOLUTE(.); + + .data : ALIGN(4) { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.sdata .sdata.* .sdata2.*) + *(.gnu.linkonce.d.*) + *(.gnu.linkonce.s.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > progmem + + PROVIDE(__global_pointer$ = _sdata + ((_edata - _sdata) / 2)); + + .bss : ALIGN(4) { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.sbss .sbss.*) + *(.gnu.linkonce.b.*) + *(.gnu.linkonce.sb.*) + *(COMMON) + . = ALIGN(32); + _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/boards/ssrc/icicle/nuttx-config/scripts/toc.ld b/boards/ssrc/icicle/nuttx-config/scripts/toc.ld new file mode 100644 index 000000000000..78e75dcc4ada --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/scripts/toc.ld @@ -0,0 +1,62 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + progmem (r) : ORIGIN = 0xACA00000, LENGTH = 6M - 64 +} + +OUTPUT_ARCH("riscv") + +EXTERN(_main_toc) + +SECTIONS +{ + .toc : { + /* The ToC */ + _toc_start = ABSOLUTE(.); + KEEP(*(.main_toc)); + /* Padd the rest */ + FILL(0xff); + . = ALIGN(8); + _toc_end = ABSOLUTE(.); + } > progmem + + /* Start of the ToC signature, appended directly after ToC */ + PROVIDE(_toc_signature = ALIGN(4)); + + .toc_sig (NOLOAD) : { + /* Make a hole for the singature */ + KEEP(*(.main_toc_sig)); + } > progmem + + .app (NOLOAD) : { + /* The application firmware payload */ + _app_start = ABSOLUTE(.); + *(.firmware) + . = ALIGN(4); + _app_end = ABSOLUTE(.); + } > progmem + + /* Start of the payload signature. This has to be in the end of the + * payload and aligned to a 4 byte boundary + */ + PROVIDE(_boot_signature = ALIGN(4)); +} diff --git a/boards/ssrc/icicle/nuttx-config/scripts/user-space.ld b/boards/ssrc/icicle/nuttx-config/scripts/user-space.ld new file mode 100755 index 000000000000..b63f419f4e58 --- /dev/null +++ b/boards/ssrc/icicle/nuttx-config/scripts/user-space.ld @@ -0,0 +1,104 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/scripts/user-space.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* NOTE: This depends on the memory.ld script having been included prior to + * this script. + */ + +OUTPUT_ARCH("riscv") + +/* section info */ + +__ld_uflash_start = ORIGIN(uflash); +__ld_uflash_end = ORIGIN(uflash)+ LENGTH(uflash); +__ld_uflash_size = LENGTH(uflash); + +__ld_usram_start = ORIGIN(usram); +__ld_usram_end = ORIGIN(usram)+ LENGTH(usram); +__ld_usram_size = LENGTH(usram); +__usrio_start = ORIGIN(usrio); +__usrio_size = LENGTH(usrio); + +EXTERN(userspace) + +SECTIONS +{ + .userspace : { + *(.userspace) + } > uflash + + .text : { + _stext = ABSOLUTE(.); + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.* .srodata .srodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > uflash + + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > uflash + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.sdata .sdata.* .sdata2.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > usram AT > uflash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.sbss .sbss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > usram + + /* 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/boards/ssrc/icicle/protected.px4board b/boards/ssrc/icicle/protected.px4board new file mode 100644 index 000000000000..112600f47eb1 --- /dev/null +++ b/boards/ssrc/icicle/protected.px4board @@ -0,0 +1,82 @@ +CONFIG_BOARD_TOOLCHAIN="riscv64-unknown-elf" +CONFIG_BOARD_ARCHITECTURE="rv64gc" +CONFIG_BOARD_CRYPTO=y +CONFIG_BOARD_PROTECTED=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS2" +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PWM_ESC=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VMOUNT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_MICRORTPS_BRIDGE=n +CONFIG_DRIVERS_PROTOCOL_SPLITTER=y +CONFIG_SYSTEMCMDS_ESC_CALIB=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_RAMP=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/ssrc/icicle/src/CMakeLists.txt b/boards/ssrc/icicle/src/CMakeLists.txt new file mode 100644 index 000000000000..d9608a4d0be3 --- /dev/null +++ b/boards/ssrc/icicle/src/CMakeLists.txt @@ -0,0 +1,101 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + mpfs_emmcsd.c + mpfs_domain.c + mpfs_composite.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + + target_include_directories(drivers_board + PRIVATE + ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/risc-v/src/opensbi/opensbi-3rdparty/include + ) + +else() + add_library(board_bus_info + i2c.cpp + spi.cpp + ) + add_dependencies(board_bus_info nuttx_context) + + add_library(drivers_board + init.c + led.c + mpfs_emmcsd.c + mpfs_ihc.c + mpfs_spinor.c + mpfs_pwm.c + manifest.c + mtd.cpp + spi_drv.cpp + ) + add_dependencies(drivers_board nuttx_context) + if (CONFIG_OPENAMP) + target_link_libraries(drivers_board + PRIVATE + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + nuttx_openamp) + else() + target_link_libraries(drivers_board + PRIVATE + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers) + endif() + + if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(drivers_board PRIVATE px4_kernel_layer) + target_compile_options(drivers_board PRIVATE -D__KERNEL__) + else() + target_link_libraries(drivers_board PRIVATE px4_layer board_bus_info) + endif() + + add_library(drivers_userspace + init_entrypt.c + mpfs_userspace.c + ) + add_dependencies(drivers_userspace nuttx_context) + +endif() diff --git a/boards/ssrc/icicle/src/board_config.h b/boards/ssrc/icicle/src/board_config.h new file mode 100644 index 000000000000..495253ad2a61 --- /dev/null +++ b/boards/ssrc/icicle/src/board_config.h @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 board_config.h + * + * Icicle internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include +#include "board_type.h" + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#define BOARD_REVISIONS {{"PolarFire MPFS250T", '1', NULL}, \ + {"PolarFire MPFS250T", '1', NULL}} + +/* Configuration ************************************************************************************/ + +/* Icicle GPIOs *************************************************************************************/ + +#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_MULTIPURPOSE_VERSION_PINS +#define GPIO_HW_VERSION_PIN1 (GPIO_BANK2 | GPIO_PIN20 | GPIO_INPUT) +#define GPIO_HW_VERSION_PIN2 (GPIO_BANK2 | GPIO_PIN21 | GPIO_INPUT) +#define GPIO_HW_VERSION_PIN3 (GPIO_BANK2 | GPIO_PIN22 | GPIO_INPUT) +#define HW_INFO_INIT_PREFIX "Icicle " + +#define GPIO_nSAFETY_SWITCH_LED_OUT (GPIO_BANK2 | GPIO_PIN26 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // GPS safety switch LED +#define GPIO_LED_SAFETY GPIO_nSAFETY_SWITCH_LED_OUT + +#define GPIO_SAFETY_SWITCH_IN (GPIO_BANK2 | GPIO_PIN25 | GPIO_INPUT) // GPS safety switch +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN + +#define TONE_ALARM_PWM_OUT_PATH "/dev/pwm1" + +/* LEDS */ +//#define GPIO_nSAFETY_SWITCH_LED_OUT (GPIO_BANK2 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 1 +#define GPIO_nLED_RED (GPIO_BANK2 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 2 +// NB: These are both yellow on Icicle board: +#define GPIO_nLED_GREEN (GPIO_BANK2 | GPIO_PIN18 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 3 +#define GPIO_nLED_BLUE (GPIO_BANK2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 4 + +//#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +//#define BOARD_OVERLOAD_LED LED_RED +//#define BOARD_ARMED_STATE_LED LED_BLUE + +/* I2C */ +#define I2C_RESET_SPEED I2C_SPEED_FAST +#define BOARD_I2C_BUS_CLOCK_INIT {I2C_SPEED_FAST, I2C_SPEED_FAST} + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS2" +#define RC_SERIAL_SINGLEWIRE + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 0 + +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; + +/* eMMC/SD */ + +#define SDIO_SLOTNO 0 +#define SDIO_MINOR 0 + +/* Battery ADC */ +#define ADC_CHANNELS (1 << 1) | (1 << 2) +#define ADC_BATTERY_VOLTAGE_CHANNEL 1 +#define ADC_BATTERY_CURRENT_CHANNEL 2 + +/* PX4 Boot image type; 0 to boot directly, 1 through SBI */ +#define INFO_BIT_USE_SBI 1 + +#ifdef CONFIG_BUILD_KERNEL +#define PX4_VENDOR_BOOT_FLAGS INFO_BIT_USE_SBI +#else +#define PX4_VENDOR_BOOT_FLAGS 0 +#endif + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +extern void board_peripheral_reset(int ms); +extern int mpfs_board_emmcsd_init(void); +extern int mpfs_board_spinor_init(struct spi_dev_s *spinor); +extern int mpfs_pwm_setup(void); +extern void board_spidev_initialize(void); +extern int board_spibus_initialize(void); +extern int board_domains_init(void); + +#ifdef CONFIG_USBDEV +extern void mpfs_usbinitialize(void); +#endif + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ssrc/icicle/src/bootloader_main.c b/boards/ssrc/icicle/src/bootloader_main.c new file mode 100644 index 000000000000..63b0e01c7071 --- /dev/null +++ b/boards/ssrc/icicle/src/bootloader_main.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include +#include "riscv_internal.h" + +extern int sercon_main(int c, char **argv); +__EXPORT void board_on_reset(int status) {} + +static void configure_pmp(void) +{ +#if 0 + // SDCARD DMA: NB! sizes must be power of 2 + const uint64_t mode_bits = 0x1Full << 56; + + // 1MB of LIM ram (bootloader RAM_START & SIZE) + uint64_t base_range = (0x08000000ull | (0x100000ull - 1ull)) >> 2; + putreg64(mode_bits | base_range, 0x20005700); + + // 4MB from the start of DDR + base_range = (0x80000000ull | (0x400000ull - 1ull)) >> 2; + putreg64(mode_bits | base_range, 0x20005708); + +#else + +#define MPFS_PMPCFG_MMC_0 (MPFS_MPUCFG_BASE + 0x700) +#define MPFS_PMPCFG_MMC_1 (MPFS_MPUCFG_BASE + 0x708) +#define MPFS_PMPCFG_MMC_2 (MPFS_MPUCFG_BASE + 0x710) +#define MPFS_PMPCFG_MMC_3 (MPFS_MPUCFG_BASE + 0x718) + +#define MPFS_PMPCFG_ETH0_0 (MPFS_MPUCFG_BASE + 0x400) +#define MPFS_PMPCFG_ETH0_1 (MPFS_MPUCFG_BASE + 0x408) +#define MPFS_PMPCFG_ETH0_2 (MPFS_MPUCFG_BASE + 0x410) +#define MPFS_PMPCFG_ETH0_3 (MPFS_MPUCFG_BASE + 0x418) +#define MPFS_PMPCFG_ETH1_0 (MPFS_MPUCFG_BASE + 0x500) +#define MPFS_PMPCFG_ETH1_1 (MPFS_MPUCFG_BASE + 0x508) +#define MPFS_PMPCFG_ETH1_2 (MPFS_MPUCFG_BASE + 0x510) +#define MPFS_PMPCFG_ETH1_3 (MPFS_MPUCFG_BASE + 0x518) + + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_0); + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_1); + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_2); + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_3); + + // Ethernet MPU, MAC 0 + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_ETH0_0); + +#endif + +} + +__EXPORT void mpfs_boardinitialize(void) +{ + _alert("Icicle bootloader\n"); + + /* this call exists to fix a weird linking issue */ + up_udelay(0); + + /* Enable clocks, TIMER is used for PX4 HRT, others are common + for many peripherals */ + modifyreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_SUBBLK_CLOCK_CR_OFFSET, 0, + ( + SYSREG_SUBBLK_CLOCK_CR_TIMER + | SYSREG_SUBBLK_CLOCK_CR_GPIO2 + | SYSREG_SUBBLK_CLOCK_CR_CFM + | SYSREG_SUBBLK_CLOCK_CR_FIC3 + ) + ); + + /* Take peripheral out of reset */ + + modifyreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_SOFT_RESET_CR_OFFSET, + ( + SYSREG_SOFT_RESET_CR_TIMER + | SYSREG_SOFT_RESET_CR_GPIO2 + | SYSREG_SOFT_RESET_CR_CFM + | SYSREG_SOFT_RESET_CR_FIC3 + | SYSREG_SOFT_RESET_CR_FPGA + ), 0); + + /* configure PMP */ + + configure_pmp(); +} + +void board_late_initialize(void) +{ +} + +int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +extern void sys_tick_handler(void); + +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/ssrc/icicle/src/hw_config.h b/boards/ssrc/icicle/src/hw_config.h new file mode 100644 index 000000000000..2c17e14673fc --- /dev/null +++ b/boards/ssrc/icicle/src/hw_config.h @@ -0,0 +1,133 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0xACA00000llu +#define FLASH_START_ADDRESS APP_LOAD_ADDRESS +#define TOC_ADDRESS FLASH_START_ADDRESS +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS3,2000000" +#define BOOT_DELAY_ADDRESS 0x00000400 +#define _FLASH_KBYTES (5 * 1024) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED + +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#define BOARD_FORCE_BL_PIN GPIO_SAFETY_SWITCH_IN + +#define BOOTLOADER_USE_TOC +#define BOOTLOADER_USE_SECURITY +#define BOOTLOADER_SIGNING_ALGORITHM CRYPTO_ED25519 +#define BOOTLOADER_VERIFY_UBOOT 0 + +#define IMAGE_FN "ssrc_icicle.bin" + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 16 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 0 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#define BOARD_PIN_CS_SPINOR (GPIO_BANK2 | GPIO_PIN15 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/ssrc/icicle/src/i2c.cpp b/boards/ssrc/icicle/src/i2c.cpp new file mode 100644 index 000000000000..c0d791bb54d5 --- /dev/null +++ b/boards/ssrc/icicle/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)), + initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)), +}; diff --git a/boards/ssrc/icicle/src/init.c b/boards/ssrc/icicle/src/init.c new file mode 100644 index 000000000000..5f8eb1f9d34a --- /dev/null +++ b/boards/ssrc/icicle/src/init.c @@ -0,0 +1,334 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 init.c + * + * Icicle-specific early startup code. This file implements the + * board_app_initializ() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + syslog(LOG_DEBUG, "board_peripheral_reset\n"); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + syslog(LOG_DEBUG, "board_on_reset\n"); +} + +/************************************************************************************ + * Name: mpfs_boardinitialize + * + * Description: + * This entry point is called early in the initialization -- after all memory + * has been configured and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +mpfs_boardinitialize(void) +{ + syslog(LOG_DEBUG, "mpfs_boardinitialize\n"); + board_autoled_initialize(); + + /* this call exists to fix a weird linking issue */ + up_udelay(0); + + /* Configure Safety button GPIO */ + mpfs_configgpio(GPIO_BTN_SAFETY); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + int ret; + + /* Power on Interfaces */ +#ifdef CONFIG_USBDEV + mpfs_usbinitialize(); +#endif + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + /* configure SPI interfaces and devices (after we determined the HW version) */ + board_spidev_initialize(); + board_spibus_initialize(); + + /* configure the DMA allocator */ + /* + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + */ +#if defined(SERIAL_HAVE_RXDMA) + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + struct timespec ts; + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)mpfs_serial_dma_poll, + NULL); +#endif + + /* initial LED state */ + + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + /* + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + */ + +#ifdef CONFIG_MMCSD + ret = mpfs_board_emmcsd_init(); + + if (ret != OK) { + led_on(LED_RED); + syslog(LOG_ERR, "ERROR: Failed to initialize SD card"); + } + +#endif /* CONFIG_MMCSD */ + +#ifdef CONFIG_MPFS_HAVE_COREPWM + /* Configure PWM peripheral interfaces */ + + ret = mpfs_pwm_setup(); + + if (ret < 0) { + syslog(LOG_ERR, "Failed to initialize CorePWM driver: %d\n", ret); + } + +#endif + +#ifdef CONFIG_FS_PROCFS + /* Mount the procfs file system */ + + ret = mount(NULL, "/proc", "procfs", 0, NULL); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to mount procfs at %s: %d\n", "/proc", ret); + } + +#endif + +#ifdef CONFIG_MTD_M25P + + ret = mpfs_board_spinor_init(mpfs_spibus_initialize(1)); + + if (ret < 0) { + return ret; + } + +#endif + +#ifdef CONFIG_I2C_EE_24XX + + struct i2c_master_s *bus1 = mpfs_i2cbus_initialize(1); /* carrier board EEPROM is on bus 1 */ + + if (!bus1) { + syslog(LOG_ERR, "ERROR: Failed to initialize I2C bus 1\n"); + return -ENODEV; + } + + ret = ee24xx_initialize(bus1, 0x51, "/dev/eeprom1", EEPROM_AT24C04, false); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to init EEPROM of Carrier board\n"); + mpfs_i2cbus_uninitialize(bus1); + return ret; + } + +#endif + +#ifdef CONFIG_MPFS_IHC_CLIENT + + /* Initialize MPFS IHC and register the net rpmsg driver. */ + if (mpfs_board_ihc_init() != 0) { + syslog(LOG_ERR, "ERROR: Failed to initialize IHC"); + } + +#ifdef CONFIG_NET_RPMSG_DRV + + ret = net_rpmsg_drv_init("mpfs-ihc", "rpnet", NET_LL_TUN); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: net_rpmsg_drv_init() failed: %d\n", ret); + } + +#endif /* CONFIG_NET_RPMSG_DRV */ + +#endif /* CONFIG_MPFS_IHC_CLIENT */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} + +int board_read_VBUS_state(void) +{ + return 0; +} + +#ifdef CONFIG_MPFS_PHYINIT + +void mpfs_phy_boardinitialize(int arg) +{ +} + +#endif diff --git a/boards/ssrc/icicle/src/init_entrypt.c b/boards/ssrc/icicle/src/init_entrypt.c new file mode 100644 index 000000000000..021a479e16f7 --- /dev/null +++ b/boards/ssrc/icicle/src/init_entrypt.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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 + +#if !defined(CONFIG_BUILD_FLAT) && !defined(__KERNEL__) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifdef CONFIG_NSH_ARCHINIT +# error "CONFIG_NSH_ARCHINIT must not be defined!" +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +void px4_userspace_init(void); +int nsh_main(int argc, char *argv[]); + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int CONFIG_INIT_ENTRYPOINT(int argc, char *argv[]) +{ + boardctl(BOARDIOC_INIT, 0); + + px4_userspace_init(); + + return nsh_main(argc, argv); +} + +#endif diff --git a/boards/ssrc/icicle/src/led.c b/boards/ssrc/icicle/src/led.c new file mode 100644 index 000000000000..ae6ada578939 --- /dev/null +++ b/boards/ssrc/icicle/src/led.c @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 led.c + * + * Icicle board LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "mpfs_gpio.h" +#include "board_config.h" + +#include +#include + +static uint32_t g_leds[] = { + GPIO_nLED_BLUE, + GPIO_nLED_RED, + GPIO_nSAFETY_SWITCH_LED_OUT, + GPIO_nLED_GREEN, +}; + +/* LED_ACTIVITY == 1, LED_BOOTLOADER == 2 */ +static bool g_led_state[3]; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_leds) / sizeof(g_leds[0])); l++) { + if (g_leds[l] != 0) { + g_led_state[l] = false; + mpfs_configgpio(g_leds[l]); + } + } +} + +static void set_led(int led, bool state) +{ + if (g_leds[led] != 0) { + g_led_state[led] = state; + mpfs_gpiowrite(g_leds[led], state); + } +} + +static bool get_led(int led) +{ + if (g_leds[led] != 0) { + return g_led_state[led]; + } + + return false; +} + +__EXPORT void led_on(int led) +{ + set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + set_led(led, !get_led(led)); +} diff --git a/boards/ssrc/icicle/src/manifest.c b/boards/ssrc/icicle/src/manifest.c new file mode 100644 index 000000000000..aba41217cad1 --- /dev/null +++ b/boards/ssrc/icicle/src/manifest.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h + + +static const px4_hw_mft_item_t hw_mft_list_v0000[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0100[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver/rev + {0x0000, hw_mft_list_v0000, arraySize(hw_mft_list_v0000)}, + {0x0100, hw_mft_list_v0100, arraySize(hw_mft_list_v0100)}, +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/ssrc/icicle/src/mpfs_composite.c b/boards/ssrc/icicle/src/mpfs_composite.c new file mode 100644 index 000000000000..832f927dec10 --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_composite.c @@ -0,0 +1,305 @@ +/**************************************************************************** + * boards/risc-v/mpfs/common/src/mpfs_composite.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "mpfs.h" + +#if defined(CONFIG_BOARDCTL_USBDEVCTRL) && defined(CONFIG_USBDEV_COMPOSITE) + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +static void *g_mschandle; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_mscclassobject + * + * Description: + * If the mass storage class driver is part of composite device, then + * its instantiation and configuration is a multi-step, board-specific + * process (See comments for usbmsc_configure below). In this case, + * board-specific logic must provide board_mscclassobject(). + * + * board_mscclassobject() is called from the composite driver. It must + * encapsulate the instantiation and configuration of the mass storage + * class and the return the mass storage device's class driver instance + * to the composite driver. + * + * Input Parameters: + * classdev - The location to return the mass storage class' device + * instance. + * + * Returned Value: + * 0 on success; a negated errno on failure + * + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +static int board_mscclassobject(int minor, + struct usbdev_devinfo_s *devinfo, + struct usbdevclass_driver_s **classdev) +{ + int ret; + + DEBUGASSERT(g_mschandle == NULL); + + /* Configure the mass storage device */ + + uinfo("Configuring with NLUNS=1\n"); + + ret = usbmsc_configure(1, &g_mschandle); + + if (ret < 0) { + uerr("ERROR: usbmsc_configure failed: %d\n", -ret); + return ret; + } + + uinfo("MSC handle=%p\n", g_mschandle); + + /* Bind the LUN(s) */ + + uinfo("Bind LUN=0 to /dev/mmcsd0\n"); + + ret = usbmsc_bindlun(g_mschandle, "/dev/mmcsd0", 0, 0, 0, false); + + if (ret < 0) { + uerr("ERROR: usbmsc_bindlun failed for LUN 1 at /dev/mmcsd0: %d\n", + ret); + usbmsc_uninitialize(g_mschandle); + g_mschandle = NULL; + return ret; + } + + /* Get the mass storage device's class object */ + + ret = usbmsc_classobject(g_mschandle, devinfo, classdev); + + if (ret < 0) { + uerr("ERROR: usbmsc_classobject failed: %d\n", -ret); + usbmsc_uninitialize(g_mschandle); + g_mschandle = NULL; + } + + return ret; +} +#endif + +/**************************************************************************** + * Name: board_mscuninitialize + * + * Description: + * Un-initialize the USB storage class driver. This is just an application + * specific wrapper aboutn usbmsc_unitialize() that is called form the + * composite device logic. + * + * Input Parameters: + * classdev - The class driver instrance previously give to the composite + * driver by board_mscclassobject(). + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +static void board_mscuninitialize(struct usbdevclass_driver_s *classdev) +{ + if (g_mschandle != NULL) { + usbmsc_uninitialize(g_mschandle); + g_mschandle = NULL; + } +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_composite_initialize + * + * Description: + * Perform architecture specific initialization of a composite USB device. + * + * Input Parameters: + * port - port number, unused + * + * Returned Value: + * OK always + * + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +int board_composite_initialize(int port) +{ + return OK; +} +#endif + +/**************************************************************************** + * Name: board_usbmsc_initialize + * + * Description: + * Perform architecture specific initialization of a mass storage USB + * device. + * + * Input Parameters: + * port - port number, unused + * + * Returned Value: + * OK always + * + ****************************************************************************/ + +int board_usbmsc_initialize(int port) +{ + return OK; +} + +/**************************************************************************** + * Name: board_composite_connect + * + * Description: + * Connect the USB composite device on the specified USB device port using + * the specified configuration. The interpretation of the configid is + * board specific. + * + * Input Parameters: + * port - The USB device port + * configid - The USB composite configuration + * + * Returned Value: + * A non-NULL handle value is returned on success. NULL is returned on + * any failure. + * + ****************************************************************************/ + +void *board_composite_connect(int port, int configid) +{ + /* Here we are composing the configuration of the usb composite device. + * The standard is to use one CDC/ACM and one USB mass storage device. + */ + + if (configid == 0) { +#ifdef CONFIG_USBMSC_COMPOSITE + struct composite_devdesc_s dev[2]; + int ifnobase = 0; + int strbase = COMPOSITE_NSTRIDS; + + /* Configure the CDC/ACM device */ + + /* Ask the cdcacm driver to fill in the constants we didn't + * know here. + */ + + cdcacm_get_composite_devdesc(&dev[0]); + + /* Overwrite and correct some values... */ + + /* The callback functions for the CDC/ACM class */ + + dev[0].classobject = cdcacm_classobject; + dev[0].uninitialize = cdcacm_uninitialize; + + /* Interfaces */ + + dev[0].devinfo.ifnobase = ifnobase; /* Offset to Interface-IDs */ + dev[0].minor = 0; /* The minor interface number */ + + /* Strings */ + + dev[0].devinfo.strbase = strbase; /* Offset to String Numbers */ + + /* Endpoints */ + + dev[0].devinfo.epno[CDCACM_EP_BULKIN_IDX] = 3; + dev[0].devinfo.epno[CDCACM_EP_BULKOUT_IDX] = 3; + dev[0].devinfo.epno[CDCACM_EP_INTIN_IDX] = 4; + + /* Count up the base numbers */ + + ifnobase += dev[0].devinfo.ninterfaces; + strbase += dev[0].devinfo.nstrings; + + /* Configure the mass storage device device */ + + /* Ask the usbmsc driver to fill in the constants we didn't + * know here. + */ + + usbmsc_get_composite_devdesc(&dev[1]); + + /* Overwrite and correct some values... */ + + /* The callback functions for the USBMSC class */ + + dev[1].classobject = board_mscclassobject; + dev[1].uninitialize = board_mscuninitialize; + + /* Interfaces */ + + dev[1].devinfo.ifnobase = ifnobase; /* Offset to Interface-IDs */ + dev[1].minor = 0; /* The minor interface number */ + + /* Strings */ + + dev[1].devinfo.strbase = strbase; /* Offset to String Numbers */ + + /* Endpoints */ + + dev[1].devinfo.epno[USBMSC_EP_BULKIN_IDX] = 1; + dev[1].devinfo.epno[USBMSC_EP_BULKOUT_IDX] = 2; + + /* Count up the base numbers */ + + ifnobase += dev[1].devinfo.ninterfaces; + strbase += dev[1].devinfo.nstrings; + + return composite_initialize(2, dev); +#else + return NULL; +#endif + + } else { + return NULL; + } +} + +#endif /* CONFIG_BOARDCTL_USBDEVCTRL && CONFIG_USBDEV_COMPOSITE */ diff --git a/boards/ssrc/icicle/src/mpfs_domain.c b/boards/ssrc/icicle/src/mpfs_domain.c new file mode 100644 index 000000000000..90e2cb9aa72b --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_domain.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + ****************************************************************************/ + +#undef NULL /* To please compiler */ + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define MPFS_DOMAIN_MAX_COUNT 4 +#define MPFS_DOMAIN_REGION_MAX_COUNT 12 + +struct mpfs_domain { + const char *domain_name; + const int *harts; + const unsigned n_of_harts; + const uintptr_t bootaddress; + const bool reset_allowed; + const bool domain_enabled; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct sbi_domain mpfs_domains[MPFS_DOMAIN_MAX_COUNT]; +static struct sbi_hartmask mpfs_masks[MPFS_DOMAIN_MAX_COUNT]; +static struct sbi_domain_memregion + mpfs_regions[MPFS_DOMAIN_REGION_MAX_COUNT + 1] = { + 0 +}; + +static const int linux_harts[] = {1, 3, 4}; +static const int px4_harts[] = {2}; + +static const struct mpfs_domain domains[] = { + { + .domain_name = "Linux-Ree-Domain", + .harts = linux_harts, + .n_of_harts = sizeof(linux_harts) / sizeof(linux_harts[0]), + .bootaddress = CONFIG_MPFS_HART3_ENTRYPOINT, + .reset_allowed = false, + .domain_enabled = true, + }, + { + .domain_name = "PX4-Ree-Domain", + .harts = px4_harts, + .n_of_harts = sizeof(px4_harts) / sizeof(px4_harts[0]), + .bootaddress = CONFIG_MPFS_HART2_ENTRYPOINT, + .reset_allowed = true, +#ifdef CONFIG_BUILD_KERNEL + .domain_enabled = true, +#else + .domain_enabled = false, +#endif + }, +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_domains_init + * + * Description: + * Initializes the domain structures from the hard coded domains table + * + * Input Parameters: + * None + * + * Returned Value: + * Zero (OK) is returned on success. + * + ****************************************************************************/ + +int board_domains_init(void) +{ + int err = -1; + unsigned i; + unsigned j; + struct sbi_domain_memregion *reg; + + /* All memory, all access */ + + sbi_domain_memregion_init(0, ~0UL, + (SBI_DOMAIN_MEMREGION_READABLE | + SBI_DOMAIN_MEMREGION_WRITEABLE | + SBI_DOMAIN_MEMREGION_EXECUTABLE), + &mpfs_regions[0]); + + /* Add to root domain */ + + sbi_domain_root_add_memregion(&mpfs_regions[0]); + + i = 1; + sbi_domain_for_each_memregion(&root, reg) { + if ((reg->flags & SBI_DOMAIN_MEMREGION_READABLE) || + (reg->flags & SBI_DOMAIN_MEMREGION_WRITEABLE) || + (reg->flags & SBI_DOMAIN_MEMREGION_EXECUTABLE)) { + continue; + } + + if (MPFS_DOMAIN_REGION_MAX_COUNT <= i) { + return SBI_EINVAL; + } + + sbi_memcpy(&mpfs_regions[i++], reg, sizeof(*reg)); + } + + /* Go through the constant configuration list */ + + for (i = 0; i < sizeof(domains) / sizeof(domains[0]); i++) { + + if (!domains[i].domain_enabled) { + continue; + } + + /* Set first hart id in the list as boot hart */ + mpfs_domains[i].boot_hartid = domains[i].harts[0]; + sbi_strncpy(mpfs_domains[i].name, + domains[i].domain_name, sizeof(mpfs_domains[i].name)); + + mpfs_domains[i].next_addr = domains[i].bootaddress; + mpfs_domains[i].next_mode = PRV_S; + mpfs_domains[i].next_arg1 = 0; + mpfs_domains[i].regions = mpfs_regions; + + for (j = 0; j < domains[i].n_of_harts; j++) { + sbi_hartmask_set_hart(domains[i].harts[j], &mpfs_masks[i]); + } + + mpfs_domains[i].possible_harts = &mpfs_masks[i]; + mpfs_domains[i].system_reset_allowed = domains[i].reset_allowed; + + /* Register the domain */ + + err = sbi_domain_register(&mpfs_domains[i], &mpfs_masks[i]); + + if (err) { + sbi_printf("%s register failed %d\n", domains[i].domain_name, err); + return err; + } + } + + return 0; +} diff --git a/boards/ssrc/icicle/src/mpfs_emmcsd.c b/boards/ssrc/icicle/src/mpfs_emmcsd.c new file mode 100644 index 000000000000..25b04399d243 --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_emmcsd.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 "mpfs_emmcsd.h" +#include "board_config.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *g_sdio_dev; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_emmcsd_init + * + * Description: + * Configure the eMMCSD driver. + * + * Returned Value: + * Zero (OK) is returned on success; A negated errno value is returned + * to indicate the nature of any failure. + * + ****************************************************************************/ + +int mpfs_board_emmcsd_init(void) +{ + int ret; + + /* Mount the SDIO-based MMC/SD block driver */ + + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + g_sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!g_sdio_dev) { + ferr("ERROR: Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, g_sdio_dev); + + if (ret != OK) { + ferr("ERROR: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + sdio_mediachange(g_sdio_dev, true); + + return OK; +} diff --git a/boards/ssrc/icicle/src/mpfs_ihc.c b/boards/ssrc/icicle/src/mpfs_ihc.c new file mode 100644 index 000000000000..3792b37130d5 --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_ihc.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. 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 "mpfs_ihc.h" +#include "board_config.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_board_ihc_init + * + * Description: + * Starts the Inter-Hart Communication (IHC) driver. + * + * Returned Value: + * Zero (OK) is returned on success; A negated errno value is returned + * to indicate any failure. + * + ****************************************************************************/ + +int mpfs_board_ihc_init(void) +{ + int ret = 0; + + /* With OpenSBI, initilization comes via mpfs_opensbi.c, not here */ + +#ifndef CONFIG_MPFS_OPENSBI + + ret = mpfs_ihc_init(); + +#endif + + return ret; +} diff --git a/boards/ssrc/icicle/src/mpfs_pwm.c b/boards/ssrc/icicle/src/mpfs_pwm.c new file mode 100644 index 000000000000..e3300da1e227 --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_pwm.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + +#include +#include +#include "mpfs_corepwm.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_pwm_setup + * + * Description: + * + * Initialize PWM and register PWM devices + * + ****************************************************************************/ + +#define PWM_DEV_NAME "/dev/pwm%d" + +int mpfs_pwm_setup(void) +{ + int npwm = 0; + char devname[sizeof(PWM_DEV_NAME)]; /* Buffer for device name */ + struct pwm_lowerhalf_s *lower_half = NULL; /* lower-half handle */ + int config_npwm = 0; /* Number of channels in use */ + + /* The underlying CorePWM driver "knows" there are up to 16 channels + * available for each timer device, so we don't have to do anything + * special here. + */ + +#ifdef CONFIG_MPFS_COREPWM0 + config_npwm++; +#endif +#ifdef CONFIG_MPFS_COREPWM1 + config_npwm++; +#endif + + for (npwm = 0; npwm < config_npwm; npwm++) { + lower_half = mpfs_corepwm_init(npwm); + + /* If we can't get the lower-half handle, skip and keep going. */ + + if (lower_half) { + /* Translate the peripheral number to a device name. */ + snprintf(devname, sizeof(devname), PWM_DEV_NAME, npwm); + pwm_register(devname, lower_half); + } + } + + return 0; +} diff --git a/boards/ssrc/icicle/src/mpfs_spinor.c b/boards/ssrc/icicle/src/mpfs_spinor.c new file mode 100644 index 000000000000..4d5dc9ce8802 --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_spinor.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 "board_config.h" +#include "hw_config.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_spinor_init + * + * Description: + * Configure the SPI NOR flash driver. + * + * Returned Value: + * Zero (OK) is returned on success; A negated errno value is returned + * to indicate the nature of any failure. + * + ****************************************************************************/ + +#ifdef CONFIG_MTD_M25P +int mpfs_board_spinor_init(struct spi_dev_s *spinor) +{ + struct mtd_dev_s *mtd, *mtd_p0, *mtd_p1; + + const char *block0 = "/dev/mtdblock0"; + const char *block1 = "/dev/mtdblock1"; + struct mtd_geometry_s geo; + int ret; + + if (!spinor) { + syslog(LOG_ERR, "EROR: FAILED to initialize SPI port 1\n"); + return -ENODEV; + } + + mtd = m25p_initialize(spinor); + + if (!mtd) { + syslog(LOG_ERR, "ERROR: Failed to bind SPI port 1 to the SPI NOR driver\n"); + return -ENODEV; + } + + // Find the MTD geometry and calculate partition sizes + if (mtd->ioctl(mtd, MTDIOC_GEOMETRY, + (unsigned long)((uintptr_t)&geo))) { + _alert("ERROR: MTD geometry unknown\n"); + return -ENODEV; + } + + unsigned all_pages = (geo.neraseblocks * geo.erasesize) / geo.blocksize; + unsigned boot_pages = BOARD_FLASH_SIZE / geo.blocksize; + + // Allocate boot partition + mtd_p0 = mtd_partition(mtd, 0, boot_pages); + + if (!mtd_p0) { + syslog(LOG_ERR, "ERROR: Failed to create boot partition\n"); + return -ENODEV; + } + + // Allocate all the rest for lfs partition + mtd_p1 = mtd_partition(mtd, boot_pages, all_pages - boot_pages); + + if (!mtd_p1) { + syslog(LOG_ERR, "ERROR: Failed to create lfs partition\n"); + return -ENODEV; + } + + ret = register_mtddriver(block0, mtd_p0, 0777, NULL); + + if (ret != 0) { + syslog(LOG_ERR, "ERROR: Failed to register MTD driver for boot partition: %d\n", ret); + return ret; + } + + ret = register_mtddriver(block1, mtd_p1, 0777, NULL); + + if (ret != 0) { + syslog(LOG_ERR, "ERROR: Failed to register MTD driver for lfs partition: %d\n", ret); + return ret; + } + + return ret; +} +#endif diff --git a/boards/ssrc/icicle/src/mpfs_userspace.c b/boards/ssrc/icicle/src/mpfs_userspace.c new file mode 100755 index 000000000000..8988d5b919fd --- /dev/null +++ b/boards/ssrc/icicle/src/mpfs_userspace.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * boards/risc-v/mpfs/common/kernel/mpfs_userspace.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include + +#if defined(CONFIG_BUILD_PROTECTED) && !defined(__KERNEL__) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +#ifndef CONFIG_NUTTX_USERSPACE +# error "CONFIG_NUTTX_USERSPACE not defined" +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* These 'addresses' of these values are setup by the linker script. + * They are not actual uint32_t storage locations! + * They are only used meaningfully in the following way: + * + * - The linker script defines, for example, the symbol_sdata. + * - The declaration extern uint32_t _sdata; makes C happy. C will believe + * that the value _sdata is the address of a uint32_t variable _data + * (it is not!). + * - We can recover the linker value then by simply taking the address of + * of _data. like: uint32_t *pdata = &_sdata; + */ + +extern uint32_t _stext; /* Start of .text */ +extern uint32_t _etext; /* End_1 of .text + .rodata */ +extern const uint32_t _eronly; /* End+1 of read only section */ +extern uint32_t _sdata; /* Start of .data */ +extern uint32_t _edata; /* End+1 of .data */ +extern uint32_t _sbss; /* Start of .bss */ +extern uint32_t _ebss; /* End+1 of .bss */ + +extern uintptr_t *__ld_usram_end; /* End+1 of user ram section */ + +/* This is the user space entry point */ + +int CONFIG_INIT_ENTRYPOINT(int argc, char *argv[]); + +const struct userspace_s userspace locate_data(".userspace") = { + /* General memory map */ + + .us_entrypoint = (main_t)CONFIG_INIT_ENTRYPOINT, + .us_textstart = (uintptr_t) &_stext, + .us_textend = (uintptr_t) &_etext, + .us_datasource = (uintptr_t) &_eronly, + .us_datastart = (uintptr_t) &_sdata, + .us_dataend = (uintptr_t) &_edata, + .us_bssstart = (uintptr_t) &_sbss, + .us_bssend = (uintptr_t) &_ebss, + + .us_heapend = (uintptr_t) &__ld_usram_end, + + /* Memory manager heap structure */ + + .us_heap = &g_mmheap, + + /* Task/thread startup routines */ + + .task_startup = nxtask_startup, + + /* Signal handler trampoline */ + + .signal_handler = up_signal_handler, + + /* User-space work queue support (declared in include/nuttx/wqueue.h) */ + +#ifdef CONFIG_LIBC_USRWORK + .work_usrstart = work_usrstart, +#endif +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +#endif /* CONFIG_BUILD_PROTECTED && !__KERNEL__ */ diff --git a/boards/ssrc/icicle/src/mtd.cpp b/boards/ssrc/icicle/src/mtd.cpp new file mode 100644 index 000000000000..fd8d20e9bbbc --- /dev/null +++ b/boards/ssrc/icicle/src/mtd.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include +// KiB BS nB +static const px4_mft_device_t i2c0 = { // 24xx64 on Base 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(1, 0x50) +}; + +static const px4_mtd_entry_t eeprom = { + .device = &i2c0, + .npart = 1, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 256 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 1, + .entries = { + &eeprom + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = { + &mtd_mft + } +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/ssrc/icicle/src/spi.cpp b/boards/ssrc/icicle/src/spi.cpp new file mode 100644 index 000000000000..d500af88b36e --- /dev/null +++ b/boards/ssrc/icicle/src/spi.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBusInternal(SPI::Bus::SPI0, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, + SPI::CS{GPIO::Bank2, GPIO::Pin8}, + SPI::DRDY{}), + /* NOTE: Not in use + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, + SPI::CS{GPIO::Bank2, GPIO::Pin11}, + SPI::DRDY{}), + */ + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, + SPI::CS{GPIO::Bank2, GPIO::Pin9}, + SPI::DRDY{GPIO::Bank2, GPIO::Pin1}) + }), + initSPIBusInternal(SPI::Bus::SPI1, { + initSPIDevice(SPIDEV_FLASH(0), + SPI::CS{GPIO::Bank2, GPIO::Pin15}, + SPI::DRDY{}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ssrc/icicle/src/spi_drv.cpp b/boards/ssrc/icicle/src/spi_drv.cpp new file mode 100644 index 000000000000..d0e59d46993f --- /dev/null +++ b/boards/ssrc/icicle/src/spi_drv.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include +#include +#include +#include + +static const px4_spi_bus_t *_spi_bus0; +static const px4_spi_bus_t *_spi_bus1; + +static inline void board_spix_select(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } + + if (devid == bus->devices[i].devid) { + // SPI select is active low, so write !selected to select the device + mpfs_gpiowrite(bus->devices[i].cs_gpio, !selected); + } + } +} + +#if defined(CONFIG_MPFS_SPI0) +__EXPORT void mpfs_spi0_select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + board_spix_select(_spi_bus0, dev, devid, selected); +} +#endif + +#if defined(CONFIG_MPFS_SPI1) +__EXPORT void mpfs_spi1_select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + board_spix_select(_spi_bus1, dev, devid, selected); +} +#endif + + +__EXPORT void board_spidev_initialize(void) +{ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } +} + +__EXPORT int board_spibus_initialize(void) +{ + // NOTE: MPFS uses 0based bus numbering + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 1: _spi_bus0 = &px4_spi_buses[i]; break; + + case 2: _spi_bus1 = &px4_spi_buses[i]; break; + } + } + + struct spi_dev_s *spi_bus0 = px4_spibus_initialize(1); + + if (!spi_bus0) { + return -ENODEV; + } + + struct spi_dev_s *spi_bus1 = px4_spibus_initialize(2); + + if (!spi_bus1) { + return -ENODEV; + } + + /* deselect all */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + SPI_SELECT(spi_bus0, px4_spi_buses[bus].devices[i].devid, false); + SPI_SELECT(spi_bus1, px4_spi_buses[bus].devices[i].devid, false); + } + } + } + + return OK; +} diff --git a/boards/ssrc/icicle/src/toc.c b/boards/ssrc/icicle/src/toc.c new file mode 100644 index 000000000000..94e87f49d317 --- /dev/null +++ b/boards/ssrc/icicle/src/toc.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. 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. + * + ****************************************************************************/ +#include +#include "board_config.h" +#include "board_type.h" + +/* Size of the signature */ + +#define SIGNATURE_SIZE PX4_SIGNATURE_SIZE(BOOTLOADER_SIGNING_ALGORITHM) + +/* ToC area boundaries */ +extern const uintptr_t _toc_start; +extern const uintptr_t _toc_end; + +#define TOC_ADDR &_toc_start +#define TOC_END ((const void *)&_toc_end) + +/* ToC signature */ +extern const uintptr_t _toc_signature; + +#define TOCSIG_ADDR ((const void *)&_toc_signature) +#define TOCSIG_END ((const void *)((const uint8_t *)TOCSIG_ADDR+SIGNATURE_SIZE)) + +/* Boot image starts at __start and ends at + * the beginning of signature, but for protected/kernel mode we don't know + * their locations. Assume binary file start and binary file end ? +*/ +extern const uintptr_t _app_start; +extern const uintptr_t _app_end; + +#define BOOT_ADDR &_app_start +#define BOOT_END ((const void *)&_app_end) + +/* Boot signature start and end are defined by the + * signature definition below +*/ +extern const uintptr_t _boot_signature; + +#define BOOTSIG_ADDR ((const void *)&_boot_signature) +#define BOOTSIG_END ((const void *)((const uint8_t *)BOOTSIG_ADDR+SIGNATURE_SIZE)) + +/* RD certifcate may follow boot signature */ + +#define RDCT_ADDR BOOTSIG_END +#define RDCT_END ((const void *)((const uint8_t*)BOOTSIG_END+sizeof(image_cert_t))) + +/* RD certificate signature follows the certificate */ + +#define RDCTSIG_ADDR RDCT_END +#define RDCTSIG_END ((const void *)((const uint8_t*)RDCTSIG_ADDR+SIGNATURE_SIZE)) + +/* The table of contents */ + +IMAGE_MAIN_TOC(6) = { + {TOC_START_MAGIC, TOC_VERSION}, + { + {"TOC", TOC_ADDR, TOC_END, 0, 1, TOC_VERIFICATION_KEY, 0, TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG0", TOCSIG_ADDR, TOCSIG_END, 0, 0, 0, 0, 0}, + {"BOOT", BOOT_ADDR, BOOT_END, 0, 3, BOOT_VERIFICATION_KEY, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE, PX4_VENDOR_BOOT_FLAGS}, + {"SIG1", BOOTSIG_ADDR, BOOTSIG_END, 0, 0, 0, 0, 0}, + {"RDCT", RDCT_ADDR, RDCT_END, 0, 5, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, + {"RDSG", RDCTSIG_ADDR, RDCTSIG_END, 0, 0, 0, 0, 0}, + }, + TOC_END_MAGIC +}; + +/* Define a signature area, just for sizing the ToC area */ + +const char _main_toc_sig[SIGNATURE_SIZE] __attribute__((section(".main_toc_sig"))); diff --git a/boards/ssrc/saluki-v1 b/boards/ssrc/saluki-v1 new file mode 160000 index 000000000000..52e74803564e --- /dev/null +++ b/boards/ssrc/saluki-v1 @@ -0,0 +1 @@ +Subproject commit 52e74803564e48efc2938e0c6577dbfe4ca46568 diff --git a/boards/ssrc/saluki-v2 b/boards/ssrc/saluki-v2 new file mode 160000 index 000000000000..44d7280b7aed --- /dev/null +++ b/boards/ssrc/saluki-v2 @@ -0,0 +1 @@ +Subproject commit 44d7280b7aedd72cb7bcd6599fea659f2ecf897f diff --git a/build.sh b/build.sh new file mode 100755 index 000000000000..0adc790d5a14 --- /dev/null +++ b/build.sh @@ -0,0 +1,95 @@ +#!/bin/bash -eux + +usage() { + set +x + echo "" + echo " usage: $0 " + echo " output-dir : directory for output artifacts" + echo " build-target : supported build targets:" + echo " px4fwupdater" + echo " pixhawk" + echo " saluki-v1_default" + echo " saluki-v1_protected" + echo " saluki-v1_amp" + echo " saluki-v1_bootloader" + echo " saluki-v2_default" + echo " saluki-v2_amp" + echo " saluki-v2_bootloader" + echo " saluki-v2_protected" + echo + exit 1 +} + +dest_dir="${1:-}" +target="${2:-}" + +if [ -z "$dest_dir" ]; then + usage +fi + +version=$(git describe --always --tags --dirty | sed 's/^v//') +script_dir=$(dirname $(realpath $0)) +dest_dir=$(realpath $1) +iname_env=tii_px4_build + +mkdir -p ${dest_dir} +pushd ${script_dir} + +build_cmd_podman_args=$(docker --version 2> /dev/null | grep -q podman && echo "--userns=keep-id" || echo "") + +build_env="docker build --build-arg UID=$(id -u) --build-arg GID=$(id -g) --pull -f ./packaging/Dockerfile.build_env -t ${iname_env} ." +build_cmd_fw="docker run --rm ${build_cmd_podman_args} -v ${script_dir}:/px4-firmware/sources ${iname_env} ./packaging/build_px4fw.sh" +build_cmd_px4fwupdater="${script_dir}/packaging/build_px4fwupdater.sh -v ${version} -i ${dest_dir}" + +# Generate build_env +if [ "${target}" != px4fwupdater ]; then + $build_env +fi + +case $target in + "px4fwupdater") + $build_cmd_px4fwupdater + ;; + "pixhawk") + $build_cmd_fw px4_fmu-v5x_ssrc + cp ${script_dir}/build/px4_fmu-v5x_ssrc/px4_fmu-v5x_ssrc.px4 ${dest_dir}/px4_fmu-v5x_ssrc-${version}.px4 + ;; + "saluki-v1_default") + $build_cmd_fw ssrc_saluki-v1_default + cp ${script_dir}/build/ssrc_saluki-v1_default/ssrc_saluki-v1_default.px4 ${dest_dir}/ssrc_saluki-v1_default-${version}.px4 + ;; + "saluki-v1_protected") + $build_cmd_fw ssrc_saluki-v1_protected + cp ${script_dir}/build/ssrc_saluki-v1_protected/ssrc_saluki-v1_protected.px4 ${dest_dir}/ssrc_saluki-v1_protected-${version}.px4 + ;; + "saluki-v1_amp") + $build_cmd_fw ssrc_saluki-v1_amp + cp ${script_dir}/build/ssrc_saluki-v1_amp/ssrc_saluki-v1_amp.bin ${dest_dir}/ssrc_saluki-v1_amp-${version}.bin + ;; + "saluki-v1_bootloader") + $build_cmd_fw ssrc_saluki-v1_bootloader + cp ${script_dir}/build/ssrc_saluki-v1_bootloader/ssrc_saluki-v1_bootloader.elf ${dest_dir}/ssrc_saluki-v1_bootloader-${version}.elf + ;; + "saluki-v2_default") + $build_cmd_fw ssrc_saluki-v2_default + cp ${script_dir}/build/ssrc_saluki-v2_default/ssrc_saluki-v2_default.px4 ${dest_dir}/ssrc_saluki-v2_default-${version}.px4 + ;; + "saluki-v2_protected") + $build_cmd_fw ssrc_saluki-v2_protected + cp ${script_dir}/build/ssrc_saluki-v2_protected/ssrc_saluki-v2_protected.px4 ${dest_dir}/ssrc_saluki-v2_protected-${version}.px4 + ;; + "saluki-v2_amp") + $build_cmd_fw ssrc_saluki-v2_amp + cp ${script_dir}/build/ssrc_saluki-v2_amp/ssrc_saluki-v2_amp.bin ${dest_dir}/ssrc_saluki-v2_amp-${version}.bin + ;; + "saluki-v2_bootloader") + $build_cmd_fw ssrc_saluki-v2_bootloader + cp ${script_dir}/build/ssrc_saluki-v2_bootloader/ssrc_saluki-v2_bootloader.elf ${dest_dir}/ssrc_saluki-v2_bootloader-${version}.elf + ;; + *) + usage + ;; +esac + +echo "Done" + diff --git a/build_sitl.sh b/build_sitl.sh new file mode 100755 index 000000000000..0e9bbc7a6255 --- /dev/null +++ b/build_sitl.sh @@ -0,0 +1,41 @@ +#!/bin/bash + +set -euxo pipefail + +if [ "$1" = "" ]; then + echo "ERROR: Package output directory not given" + echo " usage: $0 " + echo " output-dir : directory for output artifacts" + exit 1 +fi + +script_dir=$(dirname $(realpath $0)) +dest_dir=$(realpath $1) + +mkdir -p ${dest_dir} + +pushd ${script_dir} + +# Generate build_env +iname_env=tii_px4_build +docker build \ + --build-arg UID=$(id -u) \ + --build-arg GID=$(id -g) \ + --pull \ + -f ./packaging/Dockerfile.build_env -t ${iname_env} . + +# Build Saluki image +version=$(git describe --always --tags --dirty | sed 's/^v//') + +docker run \ + --rm \ + -v ${script_dir}:/px4-firmware/sources \ + ${iname_env} \ + ./packaging/build_px4_sitl.sh \ + -v ${version} \ + +mv px4_sitl_build-*.tar.gz ${dest_dir}/ +mv px4_gazebo_data-*.tar.gz ${dest_dir}/ + +echo "Done" +exit 0 diff --git a/clone_public.sh b/clone_public.sh new file mode 100755 index 000000000000..81aa48619926 --- /dev/null +++ b/clone_public.sh @@ -0,0 +1,12 @@ +#!/bin/bash + +set -euo pipefail + +while read -r repo +do + [[ "${repo}" == *saluki-v? ]] || \ + [[ "${repo}" == *pfsoc_crypto ]] || \ + [[ "${repo}" == *pfsoc_keystore ]] || \ + [[ "${repo}" == *pf_crypto ]] && continue + git submodule update --init --recursive "${repo}" +done <<< "$(git submodule status | awk '{print $2}')" diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index 17226417340d..d67b05efa908 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -166,15 +166,15 @@ function(px4_add_module) endif() if(NOT DYNAMIC) - target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf) + target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf uORB) if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL) - target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel) + target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer) set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE}) elseif(${PX4_PLATFORM} STREQUAL "qurt") target_link_libraries(${MODULE} PRIVATE px4_layer uORB) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) else() - target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB) + target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) endif() set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/packaging/Dockerfile.build_env b/packaging/Dockerfile.build_env new file mode 100644 index 000000000000..fdccbe639e29 --- /dev/null +++ b/packaging/Dockerfile.build_env @@ -0,0 +1,18 @@ +# px4-firmware builder environment +FROM ghcr.io/tiiuae/px4-firmware-builder-base:latest + +ARG UID=1000 +ARG GID=1000 + +RUN groupadd -g $GID builder && \ + useradd -m -u $UID -g $GID -g builder builder && \ + usermod -aG sudo builder && \ + echo 'builder ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers + +RUN mkdir -p /px4-firmware && chown -R builder:builder /px4-firmware +RUN mkdir -p /artifacts && chown -R builder:builder /artifacts + +USER builder + +VOLUME /px4-firmware/sources +WORKDIR /px4-firmware/sources diff --git a/packaging/Dockerfile.build_env_pre b/packaging/Dockerfile.build_env_pre new file mode 100644 index 000000000000..66c0544a570b --- /dev/null +++ b/packaging/Dockerfile.build_env_pre @@ -0,0 +1,37 @@ +# px4-firmware builder environment +FROM ros:humble-ros-base + +ENV LANG C.UTF-8 +ENV LANGUAGE C.UTF-8 +ENV LC_ALL C.UTF-8 +ENV RMW_IMPLEMENTATION rmw_fastrtps_cpp + +RUN apt-get update && apt-get install -y --no-install-recommends \ + git build-essential cmake lsb-core ninja-build \ + libboost-all-dev libeigen3-dev libgstreamer-plugins-base1.0-dev libopencv-dev \ + python3-empy python3-toml python3-numpy python3-genmsg python3-setuptools \ + python3-packaging python3-jinja2 python3-yaml openjdk-11-jre \ + gazebo libgazebo-dev \ + genromfs xxd curl unzip \ + python3-nacl python3-pip python3-future \ + ros-humble-gazebo-ros \ + ros-humble-fastrtps \ + ros-humble-rmw-fastrtps-cpp \ + fastddsgen \ + fakeroot \ + dh-make \ + debhelper \ + && pip3 install kconfiglib jsonschema \ + && rm -rf /var/lib/apt/lists/* + +RUN mkdir -p /tools + +RUN curl -LOs https://static.dev.sifive.com/dev-tools/freedom-tools/v2020.12/riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14.tar.gz && \ + tar xf riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14.tar.gz -C /tools && \ + rm -f riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14.tar.gz + +RUN curl -LOs https://developer.arm.com/-/media/Files/downloads/gnu-rm/9-2019q4/gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 && \ + tar xvf gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 -C /tools && \ + rm -f gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 + +ENV PATH=/tools/riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14/bin:/tools/gcc-arm-none-eabi-9-2019-q4-major/bin/:$PATH diff --git a/packaging/Dockerfile.sitl b/packaging/Dockerfile.sitl new file mode 100644 index 000000000000..ebb09eae7b82 --- /dev/null +++ b/packaging/Dockerfile.sitl @@ -0,0 +1,31 @@ +FROM ghcr.io/tiiuae/fog-ros-baseimage-builder:dp-4266_humble_upgrade + +RUN apt-get update && apt-get install -y --no-install-recommends \ + wget + +#RUN echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list +#RUN wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - +#RUN apt-get update && apt-get install -y --no-install-recommends \ +# libignition-transport8-dev \ +# && rm -rf /var/lib/apt/lists/* + +# Install PX4 SITL +WORKDIR /packages + +COPY bin/px4_sitl_build*.tar.gz ./px4_sitl_build.tar.gz + +RUN tar -xzf px4_sitl_build.tar.gz \ + && mv px4_sitl/bin/* /usr/bin/ \ + && mv px4_sitl/etc /px4_sitl_etc \ + && rm -rf px4_sitl_build/ \ + && rm px4_sitl_build.tar.gz + +WORKDIR /px4_sitl + +COPY px4-firmware/ssrc_config /ssrc_config +COPY px4-firmware/packaging/entrypoint.sh . + +ENV PACKAGE_NAME=px4_sitl +ENV PX4_SIM_MODEL=ssrc_fog_x + +ENTRYPOINT ["/px4_sitl/entrypoint.sh"] diff --git a/packaging/build_px4_sitl.sh b/packaging/build_px4_sitl.sh new file mode 100755 index 000000000000..a2e0bd4299aa --- /dev/null +++ b/packaging/build_px4_sitl.sh @@ -0,0 +1,79 @@ +set -eo pipefail + +usage() { + echo " +Usage: $(basename "$0") [-h] [-v version] + -- Generate tar packages from px4_sitl module. +Params: + -h Show help text. + -v Git version string +" + exit 0 +} + +check_arg() { + if [ "$(echo $1 | cut -c1)" = "-" ]; then + return 1 + else + return 0 + fi +} + +error_arg() { + echo "$0: option requires an argument -- $1" + usage +} + +mod_dir="$(realpath $(dirname $0)/..)" +VERSION="" + +while getopts "hv:" opt +do + case $opt in + h) + usage + ;; + v) + check_arg $OPTARG && VERSION=$OPTARG || error_arg $opt + ;; + \?) + usage + ;; + esac +done + +source /opt/ros/humble/setup.sh + + +# Remove old build output +rm -Rf build/px4_sitl_rtps + +# Build +DONT_RUN=1 make px4_sitl_default gazebo_ssrc_fog_x + +# tar artifacts +mkdir -p tmp_packing_dir +pushd tmp_packing_dir + +mkdir -p px4_sitl/build/px4_sitl_default +mkdir -p px4_gazebo_data/plugins +mkdir -p px4_gazebo_data/models +mkdir -p px4_gazebo_data/worlds +mkdir -p px4_gazebo_data/scripts + +find ../build/px4_sitl_default/build_gazebo/*.so -exec cp {} px4_gazebo_data/plugins \; + +cp -r ../build/px4_sitl_default/bin px4_sitl/bin +cp -r ../build/px4_sitl_default/etc px4_sitl/etc +cp -r ../Tools/simulation/gazebo/sitl_gazebo/models/asphalt_plane px4_gazebo_data/models/asphalt_plane +cp -r ../Tools/simulation/gazebo/sitl_gazebo/models/ground_plane px4_gazebo_data/models/ground_plane +cp -r ../Tools/simulation/gazebo/sitl_gazebo/models/sun px4_gazebo_data/models/sun +cp -r ../Tools/simulation/gazebo/sitl_gazebo/models/ssrc_fog_x px4_gazebo_data/models/ssrc_fog_x +cp -r ../Tools/simulation/gazebo/sitl_gazebo/worlds/empty.world px4_gazebo_data/worlds/ +cp -r ../Tools/simulation/gazebo/sitl_gazebo/worlds/empty_ssrc.world px4_gazebo_data/worlds/ +cp -r ../Tools/simulation/gazebo/sitl_gazebo/scripts/jinja_gen.py px4_gazebo_data/scripts/ +tar czvf ../px4_sitl_build-v${VERSION}.tar.gz px4_sitl +tar czvf ../px4_gazebo_data-v${VERSION}.tar.gz px4_gazebo_data + +popd +rm -Rf tmp_packing_dir diff --git a/packaging/build_px4fw.sh b/packaging/build_px4fw.sh new file mode 100755 index 000000000000..aa3e0fa2599e --- /dev/null +++ b/packaging/build_px4fw.sh @@ -0,0 +1,19 @@ +#!/bin/bash + +source /opt/ros/humble/setup.sh +export SIGNING_TOOL=Tools/cryptotools.py + +if [ -z "$1" ]; then + echo "Usage: $0 " + echo + exit 1 +else + # go through all given arguments and build them + for arg in "$@"; do + echo "BUILDING $1" + # Remove old build output + rm -Rf build/$1 + # Build + make $1 + done +fi diff --git a/packaging/build_px4fwupdater.sh b/packaging/build_px4fwupdater.sh new file mode 100755 index 000000000000..7c2da899555f --- /dev/null +++ b/packaging/build_px4fwupdater.sh @@ -0,0 +1,82 @@ +set -eo pipefail + +usage() { + echo " +Usage: $(basename "$0") [-h] [-v version] + -- Generate debian package from px4fwupdater module. +Params: + -h Show help text. + -v Git version string +" + exit 0 +} + +check_arg() { + if [ "$(echo $1 | cut -c1)" = "-" ]; then + return 1 + else + return 0 + fi +} + +error_arg() { + echo "$0: option requires an argument -- $1" + usage +} + +mod_dir="$(realpath $(dirname $0)/..)" +VERSION="" +indir="" + +while getopts "hv:i:" opt +do + case $opt in + h) + usage + ;; + v) + check_arg $OPTARG && VERSION=$OPTARG || error_arg $opt + ;; + i) + indir=$OPTARG + ;; + \?) + usage + ;; + esac +done + +set -x + +if [ -e tmp_packaging_dir ]; then + rm -Rf tmp_packaging_dir +fi + +mkdir -p tmp_packaging_dir +pushd tmp_packaging_dir + +mkdir -p ./opt/px4fwupdater/ +if [ -n "${indir}" ] +then + cp ${indir}/*.px4 ./opt/px4fwupdater/ +else + cp ${mod_dir}/build/ssrc_saluki-v1_default/*.px4 ./opt/px4fwupdater/ + cp ${mod_dir}/build/ssrc_saluki-v2_default/*.px4 ./opt/px4fwupdater/ + cp ${mod_dir}/build/px4_fmu-v5x_ssrc/*.px4 ./opt/px4fwupdater/ +fi + +mkdir DEBIAN +cp -R ${mod_dir}/packaging/debian/* DEBIAN/ +cp ${mod_dir}/packaging/px4_update_fw.sh ./opt/px4fwupdater/px4_update_fw.sh +cp ${mod_dir}/packaging/detect_ttyS.sh ./opt/px4fwupdater/detect_ttyS.sh +cp ${mod_dir}/Tools/px_uploader.py ./opt/px4fwupdater/px_uploader.py + +if [ -e ../px4fwupdater*.deb ]; then + rm -f ../px4fwupdater*.deb +fi + +sed -i "s/Version:.*/&${VERSION}/" DEBIAN/control +fakeroot dpkg-deb --build . "${indir}"/px4fwupdater_${VERSION}_amd64.deb + +popd +rm -Rf tmp_packaging_dir diff --git a/packaging/debian/control b/packaging/debian/control new file mode 100644 index 000000000000..c6db7fd753d4 --- /dev/null +++ b/packaging/debian/control @@ -0,0 +1,6 @@ +Package: px4-firmware-updater +Version: +Maintainer: Jani Paalijärvi +Architecture: amd64 +Depends: python3-serial +Description: PX4 firmware updater package. Installs included firmware to PixHawk4. diff --git a/packaging/debian/postinst b/packaging/debian/postinst new file mode 100755 index 000000000000..a9bf588e2f88 --- /dev/null +++ b/packaging/debian/postinst @@ -0,0 +1 @@ +#!/bin/bash diff --git a/packaging/debian/prerm b/packaging/debian/prerm new file mode 100755 index 000000000000..a9bf588e2f88 --- /dev/null +++ b/packaging/debian/prerm @@ -0,0 +1 @@ +#!/bin/bash diff --git a/packaging/detect_ttyS.sh b/packaging/detect_ttyS.sh new file mode 100755 index 000000000000..4bfc752535c3 --- /dev/null +++ b/packaging/detect_ttyS.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +port=`cat /proc/tty/driver/serial | grep DTR | cut -d':' -f1` +echo "${port}" + diff --git a/packaging/entrypoint.sh b/packaging/entrypoint.sh new file mode 100755 index 000000000000..22a1abdff53c --- /dev/null +++ b/packaging/entrypoint.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +source /opt/ros/humble/setup.bash + +if [ "$1" == "sim_tcp_server" ] || [ "$PX4_SIM_USE_TCP_SERVER" != "" ]; then + px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS.sim_tcp_server +else + px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS +fi diff --git a/packaging/px4_update_fw.sh b/packaging/px4_update_fw.sh new file mode 100755 index 000000000000..e58b022614c4 --- /dev/null +++ b/packaging/px4_update_fw.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +port=$(/opt/px4fwupdater/detect_ttyS.sh) + +if [ -z $port ] +then + echo "Unable to detect serial port. Exiting.." + exit 1 +fi + +systemctl is-active agent_protocol_splitter.service &> /dev/null +if [ $? -eq 0 ] +then + echo "Stopping agent_protocol_splitter before flashing" + systemctl stop agent_protocol_splitter.service + echo "Start flashing.. port /dev/ttyS${port}" + /opt/px4fwupdater/px_uploader.py \ + --port /dev/ttyS${port} \ + --baud-bootloader 2000000 \ + --baud-flightstack 57600,115200,1000000,2000000 \ + --use-protocol-splitter-format \ + /opt/px4fwupdater/px4_fmu-v5_ssrc.px4 + echo "Start agent_protocol_splitter" + systemctl start agent_protocol_splitter.service +else + echo "Start flashing.. port /dev/ttyS{port}" + /opt/px4fwupdater/px_uploader.py \ + --port /dev/ttyS${port} \ + --baud-bootloader 2000000 \ + --baud-flightstack 57600,115200,1000000,2000000 \ + --use-protocol-splitter-format \ + /opt/px4fwupdater/px4_fmu-v5_ssrc.px4 +fi diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index 11e0628a8d6a..e114c4173bd4 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -54,7 +54,7 @@ add_library(px4_platform STATIC spi.cpp ${SRCS} ) -target_link_libraries(px4_platform prebuild_targets px4_work_queue) +target_link_libraries(px4_platform prebuild_targets px4_work_queue uorb_msgs) if(NOT "${PX4_BOARD}" MATCHES "io-v2") add_subdirectory(uORB) diff --git a/platforms/common/apps.cpp.in b/platforms/common/apps.cpp.in index 11d1c50d9f97..c42d79c85d3c 100644 --- a/platforms/common/apps.cpp.in +++ b/platforms/common/apps.cpp.in @@ -2,6 +2,7 @@ #include #include #include +#include #include "apps.h" @@ -43,6 +44,7 @@ void list_builtins(apps_map_type &apps) int shutdown_main(int argc, char *argv[]) { printf("Exiting NOW.\n"); + uORB::Manager::terminate(); system_exit(0); } diff --git a/platforms/common/events.cpp b/platforms/common/events.cpp index c4d6867020c0..969cd2c51e58 100644 --- a/platforms/common/events.cpp +++ b/platforms/common/events.cpp @@ -38,7 +38,7 @@ #include #include -static orb_advert_t orb_event_pub = nullptr; +static orb_advert_t orb_event_pub = ORB_ADVERT_INVALID; static pthread_mutex_t publish_event_mutex = PTHREAD_MUTEX_INITIALIZER; static uint16_t event_sequence{events::initial_event_sequence}; @@ -57,8 +57,8 @@ void send(EventType &event) pthread_mutex_lock(&publish_event_mutex); event.event_sequence = ++event_sequence; // Set the sequence here so we're able to detect uORB queue overflows - if (orb_event_pub != nullptr) { - orb_publish(ORB_ID(event), orb_event_pub, &event); + if (orb_advert_valid(orb_event_pub)) { + orb_publish(ORB_ID(event), &orb_event_pub, &event); } else { orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH); diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index 47b3fb6de87b..6a4e00694d4d 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -58,9 +58,7 @@ #include #include -#if defined(__PX4_NUTTX) -# include -#endif // __PX4_NUTTX +#include namespace px4 { @@ -75,26 +73,31 @@ class atomic // IRQ handlers. This might not be required everywhere though. static_assert(__atomic_always_lock_free(sizeof(T), 0), "atomic is not lock-free for the given type T"); #endif // __PX4_POSIX - - atomic() = default; - explicit atomic(T value) : _value(value) {} + atomic() + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + px4_sem_init(&_lock, 0, 1); + } + } + explicit atomic(T value) : _value(value) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + px4_sem_init(&_lock, 0, 1); + } + } /** * Atomically read the current value */ inline T load() const { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); } } @@ -104,16 +107,12 @@ class atomic */ inline void store(T value) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); _value = value; - leave_critical_section(flags); + release_lock(); - } else -#endif // __PX4_NUTTX - { + } else { __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); } } @@ -124,18 +123,14 @@ class atomic */ inline T fetch_add(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T ret = _value; _value += num; - leave_critical_section(flags); + release_lock(); return ret; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); } } @@ -146,18 +141,14 @@ class atomic */ inline T fetch_sub(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T ret = _value; _value -= num; - leave_critical_section(flags); + release_lock(); return ret; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); } } @@ -168,18 +159,14 @@ class atomic */ inline T fetch_and(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; _value &= num; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); } } @@ -190,18 +177,14 @@ class atomic */ inline T fetch_xor(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; _value ^= num; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); } } @@ -212,18 +195,14 @@ class atomic */ inline T fetch_or(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; _value |= num; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); } } @@ -234,18 +213,14 @@ class atomic */ inline T fetch_nand(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T ret = _value; _value = ~(_value & num); - leave_critical_section(flags); + release_lock(); return ret; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); } } @@ -260,37 +235,48 @@ class atomic */ inline bool compare_exchange(T *expected, T desired) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); if (_value == *expected) { _value = desired; - leave_critical_section(flags); + release_lock(); return true; } else { *expected = _value; - leave_critical_section(flags); + release_lock(); return false; } - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); } } private: T _value {}; + + inline void take_lock() const { do {} while (px4_sem_wait(&_lock) != 0); } + inline void release_lock() const { px4_sem_post(&_lock); } + mutable px4_sem_t _lock; }; using atomic_int = atomic; using atomic_int32_t = atomic; + +/* On riscv64-unknown-elf atomic is not quaranteed to be lock-free + * It is unclear whether it is really required. + * An optimal solution could be atomic_flag, but it doesn't seem to be available + * Just use atomic ints for now, to be safe +*/ +#if !defined(CONFIG_ARCH_RISCV) using atomic_bool = atomic; +#else +using atomic_bool = atomic; +#endif } /* namespace px4 */ #endif /* __cplusplus */ + diff --git a/platforms/common/include/px4_platform_common/atomic_from_isr.h b/platforms/common/include/px4_platform_common/atomic_from_isr.h new file mode 100644 index 000000000000..191e969dfdf1 --- /dev/null +++ b/platforms/common/include/px4_platform_common/atomic_from_isr.h @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 atomic_from_isr.h + * + * Provides atomic integers and counters. Each method is executed atomically and thus + * can be used to prevent data races and add memory synchronization between threads. + * + * In addition to the atomicity, each method serves as a memory barrier (sequential + * consistent ordering). This means all operations that happen before and could + * potentially have visible side-effects in other threads will happen before + * the method is executed. + * + * The implementation uses the built-in methods from GCC (supported by Clang as well). + * @see https://gcc.gnu.org/onlinedocs/gcc/_005f_005fatomic-Builtins.html. + * + * @note: on ARM, the instructions LDREX and STREX might be emitted. To ensure correct + * behavior, the exclusive monitor needs to be cleared on a task switch (via CLREX). + * This happens automatically e.g. on ARMv7-M as part of an exception entry or exit + * sequence. + */ + +#pragma once + +#ifdef __cplusplus + +#include +#include + + +#if !defined(__PX4_NUTTX) + +/* For non-NuttX targets forward this to the generic atomic implementation */ +#include "atomic.h" + +namespace px4 +{ + +template +using atomic_from_isr = atomic; + +} + +#else + +/* For NuttX targets, implement an IRQ safe way for atomic data */ +#include + +namespace px4 +{ + +template +class atomic_from_isr +{ +public: + + atomic_from_isr() = default; + explicit atomic_from_isr(T value) : _value(value) {} + + /** + * Atomically read the current value + */ + inline T load() const + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + leave_critical_section(flags); + return val; + + } else { + return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomically store a value + */ + inline void store(T value) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + _value = value; + leave_critical_section(flags); + + } else { + __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomically add a number and return the previous value. + * @return value prior to the addition + */ + inline T fetch_add(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value += num; + leave_critical_section(flags); + return ret; + + } else { + return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomically substract a number and return the previous value. + * @return value prior to the substraction + */ + inline T fetch_sub(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value -= num; + leave_critical_section(flags); + return ret; + + } else { + return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic AND with a number + * @return value prior to the operation + */ + inline T fetch_and(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value &= num; + leave_critical_section(flags); + return val; + + } else { + return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic XOR with a number + * @return value prior to the operation + */ + inline T fetch_xor(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value ^= num; + leave_critical_section(flags); + return val; + + } else { + return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic OR with a number + * @return value prior to the operation + */ + inline T fetch_or(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value |= num; + leave_critical_section(flags); + return val; + + } else { + return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic NAND (~(_value & num)) with a number + * @return value prior to the operation + */ + inline T fetch_nand(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value = ~(_value & num); + leave_critical_section(flags); + return ret; + + } else { + return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic compare and exchange operation. + * This compares the contents of _value with the contents of *expected. If + * equal, the operation is a read-modify-write operation that writes desired + * into _value. If they are not equal, the operation is a read and the current + * contents of _value are written into *expected. + * @return If desired is written into _value then true is returned + */ + inline bool compare_exchange(T *expected, T desired) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + + if (_value == *expected) { + _value = desired; + leave_critical_section(flags); + return true; + + } else { + *expected = _value; + leave_critical_section(flags); + return false; + } + + } else { + return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); + } + } + +private: + T _value {}; +}; + +} /* namespace px4 */ + +#endif /* __PX4_NUTTX */ +#endif /* __cplusplus */ diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index a7c59df0b299..b0b5d4fa75b2 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -351,6 +351,8 @@ typedef enum PX4_SOC_ARCH_ID_t { PX4_SOC_ARCH_ID_BBBLUE = 0x1008, + PX4_SOC_ARCH_ID_MPFS = 0x2000, + } PX4_SOC_ARCH_ID_t; diff --git a/platforms/common/include/px4_platform_common/crypto_algorithms.h b/platforms/common/include/px4_platform_common/crypto_algorithms.h index 32a4a44f5f48..6c5d04f957f0 100644 --- a/platforms/common/include/px4_platform_common/crypto_algorithms.h +++ b/platforms/common/include/px4_platform_common/crypto_algorithms.h @@ -43,4 +43,15 @@ typedef enum { CRYPTO_XCHACHA20 = 2, CRYPTO_AES = 3, CRYPTO_RSA_OAEP = 4, + CRYPTO_ECDSA_P256 = 5, + CRYPTO_RSA_SIG = 6, /* openssl dgst -sha256 -sign */ } px4_crypto_algorithm_t; + +/* Define the expected size of the signature for signing algorithms */ + +#define PX4_SIGNATURE_SIZE(x) PX4_SIGNATURE_SIZE_(x) +#define PX4_SIGNATURE_SIZE_(x) PX4_SIGNATURE_SIZE_##x + +#define PX4_SIGNATURE_SIZE_CRYPTO_ED25519 64 +#define PX4_SIGNATURE_SIZE_CRYPTO_ECDSA_P256 64 /* Raw R+S, both 32 bytes */ +#define PX4_SIGNATURE_SIZE_CRYPTO_RSA_SIG 256 /* Assume key length of 2048 bits */ diff --git a/platforms/common/include/px4_platform_common/crypto_backend.h b/platforms/common/include/px4_platform_common/crypto_backend.h index 509393367935..bf5ea25580d8 100644 --- a/platforms/common/include/px4_platform_common/crypto_backend.h +++ b/platforms/common/include/px4_platform_common/crypto_backend.h @@ -48,6 +48,7 @@ extern "C" { */ void keystore_init(void); +void keystore_deinit(void); /* * Open a session for accessing security keys @@ -93,6 +94,13 @@ bool keystore_put_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *ke void crypto_init(void); +/* + * De-initialize hw level crypto + * This may be called to shut down hw level crypto + */ + +void crypto_deinit(void); + /* * Open a session for performing crypto functions * algorithm: The crypto algorithm to be used in this session diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 913481bfeeb3..6d89a51cf44a 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -116,10 +116,21 @@ __END_DECLS #define M_LOG10E_F 0.43429448f #define M_LN2_F 0.69314718f #define M_LN10_F 2.30258509f + +#ifndef M_PI_F #define M_PI_F 3.14159265f +#endif + #define M_TWOPI_F 6.28318531f + +#ifndef M_PI_2_F #define M_PI_2_F 1.57079632f +#endif + +#ifndef M_PI_4_F #define M_PI_4_F 0.78539816f +#endif + #define M_3PI_4_F 2.35619449f #define M_SQRTPI_F 1.77245385f #define M_1_PI_F 0.31830989f diff --git a/platforms/common/include/px4_platform_common/mmap.h b/platforms/common/include/px4_platform_common/mmap.h new file mode 100644 index 000000000000..7bcf2f095f8b --- /dev/null +++ b/platforms/common/include/px4_platform_common/mmap.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#if defined(__PX4_NUTTX) +#include +#endif + +#if defined (__PX4_NUTTX) && defined(CONFIG_BUILD_KERNEL) + +/* For size_t */ +#include + +__BEGIN_DECLS + +void *px4_mmap(void *start, size_t length, int prot, int flags, int fd, off_t offset); +int px4_munmap(void *start, size_t length); + +__END_DECLS + +#else +#define px4_mmap mmap +#define px4_munmap munmap +#endif diff --git a/platforms/common/include/px4_platform_common/posix.h b/platforms/common/include/px4_platform_common/posix.h index ac99e676e2a5..7b6e417afd86 100644 --- a/platforms/common/include/px4_platform_common/posix.h +++ b/platforms/common/include/px4_platform_common/posix.h @@ -45,19 +45,21 @@ #include #include #include +#include #include "sem.h" #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 +typedef orb_poll_struct_t px4_pollfd_struct_t; +typedef orb_pollevent_t px4_pollevent_t; +#define px4_poll orb_poll + #ifdef __PX4_NUTTX #include -typedef struct pollfd px4_pollfd_struct_t; -typedef pollevent_t px4_pollevent_t; - #if defined(__cplusplus) #define _GLOBAL :: #else @@ -68,7 +70,6 @@ typedef pollevent_t px4_pollevent_t; #define px4_ioctl _GLOBAL ioctl #define px4_write _GLOBAL write #define px4_read _GLOBAL read -#define px4_poll _GLOBAL poll #define px4_access _GLOBAL access #define px4_getpid _GLOBAL getpid @@ -83,19 +84,6 @@ typedef pollevent_t px4_pollevent_t; __BEGIN_DECLS -typedef short px4_pollevent_t; - -typedef struct { - /* This part of the struct is POSIX-like */ - int fd; /* The descriptor being polled */ - px4_pollevent_t events; /* The input event flags */ - px4_pollevent_t revents; /* The output event flags */ - - /* Required for PX4 compatibility */ - px4_sem_t *sem; /* Pointer to semaphore used to post output event */ - void *priv; /* For use by drivers */ -} px4_pollfd_struct_t; - #ifndef POLLIN #define POLLIN (0x01) #endif @@ -105,7 +93,6 @@ __EXPORT int px4_close(int fd); __EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen); __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); -__EXPORT int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout); __EXPORT int px4_access(const char *pathname, int mode); __EXPORT px4_task_t px4_getpid(void); diff --git a/platforms/common/include/px4_platform_common/px4_mtd.h b/platforms/common/include/px4_platform_common/px4_mtd.h index 7313bd78a93f..d7c356176621 100644 --- a/platforms/common/include/px4_platform_common/px4_mtd.h +++ b/platforms/common/include/px4_platform_common/px4_mtd.h @@ -37,6 +37,9 @@ __BEGIN_DECLS #define MAX_MTD_INSTANCES 5u +// Forward decalarations +FAR struct i2c_master_s; + // The data needed to interface with mtd device's typedef struct { diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index f2a66bb8983c..5381e76a5f14 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -43,6 +43,10 @@ #include #include +#ifdef __PX4_NUTTX +# include +#endif + namespace px4 { @@ -84,9 +88,16 @@ class WorkQueue : public IntrusiveSortedListNode #ifdef __PX4_NUTTX // In NuttX work can be enqueued from an ISR +#ifdef CONFIG_BUILD_FLAT void work_lock() { _flags = enter_critical_section(); } void work_unlock() { leave_critical_section(_flags); } irqstate_t _flags; +#else + // For non-flat targets, work is enqueued by user threads as well + void work_lock() { _atomic.start(); } + void work_unlock() { _atomic.finish(); } + atomic_block _atomic; +#endif #else // loop as the wait may be interrupted by a signal void work_lock() { do {} while (px4_sem_wait(&_qlock) != 0); } diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 94614bf5d824..f1c8def4b654 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -50,13 +50,13 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 3150, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1}; -static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2}; -static constexpr wq_config_t SPI2{"wq:SPI2", 2336, -3}; -static constexpr wq_config_t SPI3{"wq:SPI3", 2336, -4}; -static constexpr wq_config_t SPI4{"wq:SPI4", 2336, -5}; -static constexpr wq_config_t SPI5{"wq:SPI5", 2336, -6}; -static constexpr wq_config_t SPI6{"wq:SPI6", 2336, -7}; +static constexpr wq_config_t SPI0{"wq:SPI0", 2568, -1}; +static constexpr wq_config_t SPI1{"wq:SPI1", 2568, -2}; +static constexpr wq_config_t SPI2{"wq:SPI2", 2568, -3}; +static constexpr wq_config_t SPI3{"wq:SPI3", 2568, -4}; +static constexpr wq_config_t SPI4{"wq:SPI4", 2568, -5}; +static constexpr wq_config_t SPI5{"wq:SPI5", 2568, -6}; +static constexpr wq_config_t SPI6{"wq:SPI6", 2568, -7}; static constexpr wq_config_t I2C0{"wq:I2C0", 2336, -8}; static constexpr wq_config_t I2C1{"wq:I2C1", 2336, -9}; @@ -76,17 +76,17 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; -static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21}; -static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22}; -static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23}; -static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24}; -static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25}; -static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26}; -static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27}; -static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28}; -static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29}; -static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30}; -static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31}; +static constexpr wq_config_t ttyS0{"wq:ttyS0", 1732, -21}; +static constexpr wq_config_t ttyS1{"wq:ttyS1", 1732, -22}; +static constexpr wq_config_t ttyS2{"wq:ttyS2", 1732, -23}; +static constexpr wq_config_t ttyS3{"wq:ttyS3", 1732, -24}; +static constexpr wq_config_t ttyS4{"wq:ttyS4", 1732, -25}; +static constexpr wq_config_t ttyS5{"wq:ttyS5", 1732, -26}; +static constexpr wq_config_t ttyS6{"wq:ttyS6", 1732, -27}; +static constexpr wq_config_t ttyS7{"wq:ttyS7", 1732, -28}; +static constexpr wq_config_t ttyS8{"wq:ttyS8", 1732, -29}; +static constexpr wq_config_t ttyS9{"wq:ttyS9", 1732, -30}; +static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1732, -31}; static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32}; static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 8e733b5ee4c1..84480d3ca7a6 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -55,7 +55,7 @@ static LogHistory g_log_history; #endif -static orb_advert_t orb_log_message_pub = nullptr; +static orb_advert_t orb_log_message_pub = ORB_ADVERT_INVALID; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" }; @@ -178,7 +178,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char } /* publish an orb log message */ - if (level >= _PX4_LOG_LEVEL_INFO && orb_log_message_pub) { //publish all messages + if (level >= _PX4_LOG_LEVEL_INFO && orb_advert_valid(orb_log_message_pub)) { //publish all messages log_message_s log_message; @@ -199,7 +199,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char va_end(argptr); log_message.text[max_length - 1] = 0; //ensure 0-termination log_message.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message); + orb_publish(ORB_ID(log_message), &orb_log_message_pub, &log_message); } } diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index c151d051c747..36378b61268e 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -34,9 +34,7 @@ # this includes the generated topics directory include_directories(${CMAKE_CURRENT_BINARY_DIR}) -set(SRCS) - -set(SRCS_COMMON +px4_add_library(uORB ORBSet.hpp Publication.hpp PublicationMulti.hpp @@ -50,55 +48,17 @@ set(SRCS_COMMON uORBCommon.hpp uORBCommunicator.hpp uORBManager.hpp + uORBManager.cpp uORBUtils.cpp uORBUtils.hpp - uORBDeviceMaster.hpp - uORBDeviceNode.hpp - ) - -set(SRCS_KERNEL - uORBDeviceMaster.cpp uORBDeviceNode.cpp - uORBManager.cpp - ) - -set(SRCS_USER - uORBManagerUsr.cpp + uORBDeviceNode.hpp ) -if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") - # Kernel side library in nuttx kernel/protected build - px4_add_library(uORB_kernel - ${SRCS_COMMON} - ${SRCS_KERNEL} - ) - target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm) - target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__) - - # User side library in nuttx kernel/protected build - px4_add_library(uORB - ${SRCS_COMMON} - ${SRCS_USER} - ) -elseif("${PX4_PLATFORM}" MATCHES "nuttx") - - # Library for NuttX (flat build, posix...) - px4_add_library(uORB - ${SRCS_COMMON} - ${SRCS_KERNEL} - ) - target_link_libraries(uORB PRIVATE cdev nuttx_mm) -else() - - # Library for all other targets (posix...) - px4_add_library(uORB - ${SRCS_COMMON} - ${SRCS_KERNEL} - ) - target_link_libraries(uORB PRIVATE cdev) +if ("${PX4_PLATFORM}" MATCHES "posix") + target_link_libraries(uORB PRIVATE rt) endif() -target_link_libraries(uORB PRIVATE uorb_msgs) target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) if(PX4_TESTING) diff --git a/platforms/common/uORB/IndexedStack.hpp b/platforms/common/uORB/IndexedStack.hpp new file mode 100644 index 000000000000..a17a7829dbec --- /dev/null +++ b/platforms/common/uORB/IndexedStack.hpp @@ -0,0 +1,143 @@ +#pragma once + +#include + +/* + * This is a single-linked list/stack which items can be accessed via handle of + * type H. The handle can be either an index (int8_t) to a pre-allocated list, + * or a direct pointer to the item. The items should have a "next" member + * variable of the intended handle type. +*/ + +template class IndexedStack; + +/* The list can be used via the IndexedStackHandle element */ + +template +class IndexedStackHandle +{ +public: + IndexedStackHandle(class IndexedStack &stack) : _stack(stack) {} + + void push(H handle) { _stack.push(handle);} + H pop() { return _stack.pop(); } + void push_free(H handle) { _stack.push_free(handle); } + H pop_free() { return _stack.pop_free(); } + bool rm(H handle) { return _stack.rm(handle); } + H head() {return _stack._head;} + H next(H handle) {return peek(handle)->next;} + bool empty() {return !_stack.handle_valid(_stack.head());} + T *peek(H handle) { return _stack.handle_valid(handle) ? _stack.peek(handle) : nullptr; } + bool handle_valid(H handle) {return _stack.handle_valid(handle); } + +private: + + class IndexedStack &_stack; +}; + + +/* The actual implementation of the list. This supports two types of handles; + * void * for NuttX flat build / single process environment + * int8_t for share memory / communication between processes + */ + +template +class IndexedStack +{ +public: + friend class IndexedStackHandle; + + IndexedStack() + { + clear_handle(_head); + clear_handle(_free_head); + init_freelist(_free_head); + } + +private: + void push(H handle) { push(handle, _head);} + H pop() { return pop(_head); } + void push_free(H handle) { push(handle, _free_head); } + H pop_free() { return pop(_free_head); } + + bool rm(H handle) + { + H p = _head; + H r; clear_handle(r); + + if (!handle_valid(handle) || + !handle_valid(p)) { + return r; + } + + if (p == handle) { + // remove the first item + T *item = peek(_head); + _head = item->next; + r = p; + + } else { + while (handle_valid((r = peek(p)->next))) { + if (r == handle) { + T *prev = peek(p); + T *item = peek(r); + // remove the item + prev->next = item->next; + break; + } + + p = r; + } + } + + return handle_valid(r) ? true : false; + } + + /* Helper functions for constructor; initialize the list of free items */ + void init_freelist(int8_t handle) + { + /* Add all items into free list */ + IndexedStackHandle self(*this); + + for (int8_t i = 0; i < S ; i++) { + self.push_free(i); + } + + _free_head = S - 1; + } + + void init_freelist(void *handle) {} + + /* Push & pop internal implementations for actual and freelist */ + void push(H handle, H &head) + { + if (handle_valid(handle)) { + T *item = peek(handle); + item->next = head; + head = handle; + } + } + + H pop(H &head) + { + H ret = head; + + if (handle_valid(ret)) { + T *item = peek(head); + head = item->next; + } + + return ret; + } + + T *peek(int8_t handle) { return &_item[handle]; } + T *peek(void *handle) { return static_cast(handle); } + static bool handle_valid(int8_t handle) { return handle >= 0; } + static bool handle_valid(void *handle) { return handle != nullptr; } + static void clear_handle(int8_t &x) { x = -1; }; + static void clear_handle(void *&x) { x = nullptr; }; + + H _head; + H _free_head; + T _item[S]; +}; diff --git a/platforms/common/uORB/ORBSet.hpp b/platforms/common/uORB/ORBSet.hpp index 28e283c7b9e7..118284308852 100644 --- a/platforms/common/uORB/ORBSet.hpp +++ b/platforms/common/uORB/ORBSet.hpp @@ -33,12 +33,14 @@ #pragma once +#include + class ORBSet { public: struct Node { Node *next{nullptr}; - const char *node_name{nullptr}; + orb_advert_t handle{ORB_ADVERT_INVALID}; }; ORBSet() = default; @@ -49,8 +51,6 @@ class ORBSet unlinkNext(_top); if (_top->next == nullptr) { - free((void *)_top->node_name); - _top->node_name = nullptr; delete _top; _top = nullptr; @@ -58,7 +58,7 @@ class ORBSet } } - void insert(const char *node_name) + void insert(const orb_advert_t &handle) { Node **p; @@ -79,36 +79,31 @@ class ORBSet } _end->next = nullptr; - _end->node_name = strdup(node_name); + _end->handle = handle; } - bool find(const char *node_name) + orb_advert_t find(const char *node_name) { Node *p = _top; while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return true; + if (strcmp(uORB::DeviceNode::get_name(p->handle), node_name) == 0) { + return p->handle; } p = p->next; } - return false; + return ORB_ADVERT_INVALID; } bool erase(const char *node_name) { Node *p = _top; - if (_top && (strcmp(_top->node_name, node_name) == 0)) { + if (_top && (strcmp(uORB::DeviceNode::get_name(_top->handle), node_name) == 0)) { p = _top->next; - if (_top->node_name) { - free((void *)_top->node_name); - _top->node_name = nullptr; - } - delete _top; _top = p; @@ -120,7 +115,7 @@ class ORBSet } while (p->next) { - if (strcmp(p->next->node_name, node_name) == 0) { + if (strcmp(uORB::DeviceNode::get_name(p->next->handle), node_name) == 0) { unlinkNext(p); return true; } @@ -142,11 +137,6 @@ class ORBSet a->next = b->next; - if (b->node_name) { - free((void *)b->node_name); - b->node_name = nullptr; - } - delete b; b = nullptr; } diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index 7547e3b7faf3..9f7763c033eb 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -70,19 +70,19 @@ class PublicationBase { public: - bool advertised() const { return _handle != nullptr; } + bool advertised() const { return orb_advert_valid(_handle); } bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); } - orb_id_t get_topic() const { return get_orb_meta(_orb_id); } + orb_id_t get_topic() const { return _meta; } protected: - PublicationBase(ORB_ID id) : _orb_id(id) {} + PublicationBase(ORB_ID id) : _meta(get_orb_meta(id)) {} ~PublicationBase() { - if (_handle != nullptr) { + if (orb_advert_valid(_handle)) { // don't automatically unadvertise queued publications (eg vehicle_command) if (Manager::orb_get_queue_size(_handle) == 1) { unadvertise(); @@ -90,8 +90,8 @@ class PublicationBase } } - orb_advert_t _handle{nullptr}; - const ORB_ID _orb_id; + orb_advert_t _handle{ORB_ADVERT_INVALID}; + const orb_id_t _meta; }; /** diff --git a/platforms/common/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp index bc275940b604..3510013880cc 100644 --- a/platforms/common/uORB/PublicationMulti.hpp +++ b/platforms/common/uORB/PublicationMulti.hpp @@ -89,7 +89,7 @@ class PublicationMulti : public PublicationBase advertise(); } - return (orb_publish(get_topic(), _handle, &data) == PX4_OK); + return (Manager::orb_publish(get_topic(), _handle, &data) == PX4_OK); } int get_instance() diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 9c17cab378d2..6fe7ffb1ee44 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -36,26 +36,24 @@ * */ +#include #include "Subscription.hpp" -#include +#include "uORBManager.hpp" namespace uORB { -bool Subscription::subscribe() +bool Subscription::subscribe(bool advertise) { - // check if already subscribed - if (_node != nullptr) { + if (orb_advert_valid(_node)) { return true; } - if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) { - unsigned initial_generation; - void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation); + if (_orb_id != ORB_ID::INVALID) { + _node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &_last_generation, advertise); - if (node) { - _node = node; - _last_generation = initial_generation; + if (orb_advert_valid(_node)) { + _advertiser = advertise; return true; } } @@ -65,22 +63,30 @@ bool Subscription::subscribe() void Subscription::unsubscribe() { - if (_node != nullptr) { - uORB::Manager::orb_remove_internal_subscriber(_node); + if (orb_advert_valid(_node)) { + uORB::Manager::orb_remove_internal_subscriber(_node, _advertiser); } - _node = nullptr; _last_generation = 0; } bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { - // if desired new instance exists, unsubscribe from current + // Subscribe to the new existing node + unsigned generation; + + if (orb_exists(get_topic(), instance) != PX4_OK) { + return false; + } + + orb_advert_t new_node = uORB::Manager::orb_add_internal_subscriber(_orb_id, instance, &generation, false); + + if (orb_advert_valid(new_node)) { unsubscribe(); + _node = new_node; _instance = instance; - subscribe(); + _last_generation = generation; return true; } diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 71c8809ad229..6850e280d5e3 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -45,13 +45,10 @@ #include #include "uORBManager.hpp" -#include "uORBUtils.hpp" namespace uORB { -class SubscriptionCallback; - // Base subscription wrapper class class Subscription { @@ -107,30 +104,18 @@ class Subscription return *this; } - ~Subscription() + virtual ~Subscription() { unsubscribe(); } - bool subscribe(); + bool subscribe(bool advertise = false); void unsubscribe(); - bool valid() const { return _node != nullptr; } + bool valid() const { return orb_advert_valid(_node); } bool advertised() { - if (valid()) { - return Manager::is_advertised(_node); - } - - // try to initialize - if (subscribe()) { - // check again if valid - if (valid()) { - return Manager::is_advertised(_node); - } - } - - return false; + return Manager::has_publisher(_orb_id, _instance); } /** @@ -186,16 +171,18 @@ class Subscription protected: friend class SubscriptionCallback; + friend class SubscriptionPollable; friend class SubscriptionCallbackWorkItem; - void *get_node() { return _node; } + orb_advert_t &get_node() { return _node; } - void *_node{nullptr}; + orb_advert_t _node{ORB_ADVERT_INVALID}; unsigned _last_generation{0}; /**< last generation the subscriber has seen */ ORB_ID _orb_id{ORB_ID::INVALID}; uint8_t _instance{0}; + bool _advertiser{false}; }; // Subscription wrapper class with data diff --git a/platforms/common/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp index 7ec29e329b6e..d1919fb67e24 100644 --- a/platforms/common/uORB/SubscriptionBlocking.hpp +++ b/platforms/common/uORB/SubscriptionBlocking.hpp @@ -105,7 +105,7 @@ class SubscriptionBlocking : public SubscriptionCallback */ bool updatedBlocking(uint32_t timeout_us = 0) { - if (!_registered) { + if (!registered()) { registerCallback(); } @@ -129,7 +129,11 @@ class SubscriptionBlocking : public SubscriptionCallback // Calculate an absolute time in the future struct timespec ts; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + px4_clock_gettime(CLOCK_MONOTONIC, &ts); +#else px4_clock_gettime(CLOCK_REALTIME, &ts); +#endif uint64_t nsecs = ts.tv_nsec + (timeout_us * 1000); static constexpr unsigned billion = (1000 * 1000 * 1000); ts.tv_sec += nsecs / billion; diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 942a9fbe17d3..b1fe664e4385 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -39,14 +39,14 @@ #pragma once #include -#include #include +#include namespace uORB { // Subscription wrapper class with callbacks on new publications -class SubscriptionCallback : public SubscriptionInterval, public ListNode +class SubscriptionCallback : public SubscriptionInterval { public: /** @@ -68,36 +68,29 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNode -#include - -#include "uORBDeviceNode.hpp" -#include "uORBManager.hpp" -#include "uORBUtils.hpp" +#include +#include +#include #include "Subscription.hpp" -#include - namespace uORB { @@ -83,9 +79,9 @@ class SubscriptionInterval SubscriptionInterval() : _subscription{nullptr} {} - ~SubscriptionInterval() = default; + virtual ~SubscriptionInterval() = default; - bool subscribe() { return _subscription.subscribe(); } + bool subscribe(bool create = false) { return _subscription.subscribe(create); } void unsubscribe() { _subscription.unsubscribe(); } bool advertised() { return _subscription.advertised(); } diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 7853e1cfe085..e9f52ffd075a 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -40,7 +40,7 @@ #include "uORBManager.hpp" #include "uORBCommon.hpp" - +#include "Publication.hpp" #include #include @@ -50,11 +50,11 @@ #include #endif -static uORB::DeviceMaster *g_dev = nullptr; +static bool initialized = false; int uorb_start(void) { - if (g_dev != nullptr) { + if (initialized) { PX4_WARN("already loaded"); /* user wanted to start uorb, its already running, no error */ return 0; @@ -65,51 +65,13 @@ int uorb_start(void) return -ENOMEM; } -#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) - /* create the driver */ - g_dev = uORB::Manager::get_instance()->get_device_master(); - - if (g_dev == nullptr) { - return -errno; - } - -#endif - + initialized = true; return OK; } -int uorb_status(void) +int orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout) { -#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) - - if (g_dev != nullptr) { - g_dev->printStatistics(); - - } else { - PX4_INFO("uorb is not running"); - } - -#else - boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_STATUS); -#endif - return OK; -} - -int uorb_top(char **topic_filter, int num_filters) -{ -#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) - - if (g_dev != nullptr) { - g_dev->showTop(topic_filter, num_filters); - - } else { - PX4_INFO("uorb is not running"); - } - -#else - boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_TOP); -#endif - return OK; + return uORB::Manager::get_instance()->orb_poll(fds, nfds, timeout); } orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) @@ -135,63 +97,63 @@ orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const vo int orb_unadvertise(orb_advert_t handle) { - return uORB::Manager::get_instance()->orb_unadvertise(handle); + return uORB::Manager::orb_unadvertise(handle); } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +int orb_publish(const struct orb_metadata *meta, orb_advert_t *handle, const void *data) { - return uORB::Manager::get_instance()->orb_publish(meta, handle, data); + return uORB::Manager::orb_publish(meta, *handle, data); } -int orb_subscribe(const struct orb_metadata *meta) +orb_sub_t orb_subscribe(const struct orb_metadata *meta) { return uORB::Manager::get_instance()->orb_subscribe(meta); } -int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +orb_sub_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance); } -int orb_unsubscribe(int handle) +int orb_unsubscribe(orb_sub_t handle) { return uORB::Manager::get_instance()->orb_unsubscribe(handle); } -int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer) { - return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer); + return uORB::Manager::orb_copy(meta, handle, buffer); } -int orb_check(int handle, bool *updated) +int orb_check(orb_sub_t handle, bool *updated) { return uORB::Manager::get_instance()->orb_check(handle, updated); } int orb_exists(const struct orb_metadata *meta, int instance) { - return uORB::Manager::get_instance()->orb_exists(meta, instance); + return uORB::Manager::orb_exists(meta, instance); } int orb_group_count(const struct orb_metadata *meta) { unsigned instance = 0; - while (uORB::Manager::get_instance()->orb_exists(meta, instance) == OK) { + while (orb_exists(meta, instance) == OK) { ++instance; }; return instance; } -int orb_set_interval(int handle, unsigned interval) +int orb_set_interval(orb_sub_t handle, unsigned interval) { - return uORB::Manager::get_instance()->orb_set_interval(handle, interval); + return uORB::Manager::orb_set_interval(handle, interval); } -int orb_get_interval(int handle, unsigned *interval) +int orb_get_interval(orb_sub_t handle, unsigned *interval) { - return uORB::Manager::get_instance()->orb_get_interval(handle, interval); + return uORB::Manager::orb_get_interval(handle, interval); } const char *orb_get_c_type(unsigned char short_type) diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index c96f3c08d45d..03d6f3c67cf9 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -56,6 +56,7 @@ struct orb_metadata { typedef const struct orb_metadata *orb_id_t; + /** * Maximum number of multi topic instances. This must be <= 10 (because it's the last char of the node path) */ @@ -114,20 +115,54 @@ typedef const struct orb_metadata *orb_id_t; __BEGIN_DECLS int uorb_start(void); -int uorb_status(void); -int uorb_top(char **topic_filter, int num_filters); /** * ORB topic advertiser handle. - * - * Advertiser handles are global; once obtained they can be shared freely - * and do not need to be closed or released. - * - * This permits publication from interrupt context and other contexts where - * a file-descriptor-based handle would not otherwise be in scope for the - * publisher. */ -typedef void *orb_advert_t; + +typedef struct { + void *node; +#ifndef CONFIG_BUILD_FLAT + void *data; +#endif + size_t data_size; +} orb_advert_t; + +/** + * ORB topic subscriber handle. + */ + +typedef void *orb_sub_t; + +/** + * Helper functions to initialize and check the handles + */ + +static inline bool orb_advert_valid(orb_advert_t handle) {return handle.node != NULL;} +#ifndef CONFIG_BUILD_FLAT +static const orb_advert_t ORB_ADVERT_INVALID = {NULL, NULL, 0}; +#else +static const orb_advert_t ORB_ADVERT_INVALID = {NULL, 0}; +#endif +static inline bool orb_sub_valid(orb_sub_t handle) {return handle != NULL;} +static const orb_sub_t ORB_SUB_INVALID = NULL; + +/** + * orb_poll struct + */ + +typedef short orb_pollevent_t; +typedef struct { + /* This part of the struct is POSIX-like */ + orb_sub_t fd; /* The polling subscriber handle */ + orb_pollevent_t events; /* The input event flags */ + orb_pollevent_t revents; /* The output event flags */ +} orb_poll_struct_t; + +/** + * @see uORB::Manager::orb_poll() + */ +extern int orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout) __EXPORT; /** * @see uORB::Manager::orb_advertise() @@ -159,7 +194,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT; /** * @see uORB::Manager::orb_publish() */ -extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t *handle, const void *data) __EXPORT; /** * Advertise as the publisher of a topic. @@ -172,15 +207,15 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance) { - if (!*handle) { + if (!orb_advert_valid(*handle)) { *handle = orb_advertise_multi(meta, data, instance); - if (*handle) { + if (orb_advert_valid(*handle)) { return 0; } } else { - return orb_publish(meta, *handle, data); + return orb_publish(meta, handle, data); } return -1; @@ -189,27 +224,27 @@ static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t /** * @see uORB::Manager::orb_subscribe() */ -extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +extern orb_sub_t orb_subscribe(const struct orb_metadata *meta) __EXPORT; /** * @see uORB::Manager::orb_subscribe_multi() */ -extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; +extern orb_sub_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; /** * @see uORB::Manager::orb_unsubscribe() */ -extern int orb_unsubscribe(int handle) __EXPORT; +extern int orb_unsubscribe(orb_sub_t handle) __EXPORT; /** * @see uORB::Manager::orb_copy() */ -extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT; +extern int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer) __EXPORT; /** * @see uORB::Manager::orb_check() */ -extern int orb_check(int handle, bool *updated) __EXPORT; +extern int orb_check(orb_sub_t handle, bool *updated) __EXPORT; /** * @see uORB::Manager::orb_exists() @@ -227,12 +262,12 @@ extern int orb_group_count(const struct orb_metadata *meta) __EXPORT; /** * @see uORB::Manager::orb_set_interval() */ -extern int orb_set_interval(int handle, unsigned interval) __EXPORT; +extern int orb_set_interval(orb_sub_t handle, unsigned interval) __EXPORT; /** * @see uORB::Manager::orb_get_interval() */ -extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; +extern int orb_get_interval(orb_sub_t handle, unsigned *interval) __EXPORT; /** * Returns the C type string from a short type in o_fields metadata, or nullptr diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 7cc5c9bd3eb0..d3a8461d116f 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -31,6 +31,8 @@ * ****************************************************************************/ +#include + #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" @@ -43,10 +45,177 @@ #endif /* CONFIG_ORB_COMMUNICATOR */ #if defined(__PX4_NUTTX) +#include #include +#include +#endif + +#include +#include +#include + +// This is a speed optimization for nuttx flat build +#ifdef CONFIG_BUILD_FLAT +#define ATOMIC_ENTER irqstate_t flags = px4_enter_critical_section() +#define ATOMIC_LEAVE px4_leave_critical_section(flags) +#else +#define ATOMIC_ENTER lock() +#define ATOMIC_LEAVE unlock() +#endif + +// Every subscriber thread has it's own list of cached subscriptions +uORB::DeviceNode::MappingCache::MappingCacheListItem *uORB::DeviceNode::MappingCache::g_cache = + nullptr; + +// This lock protects the subscription cache list from concurrent accesses by the threads in the same process +px4_sem_t uORB::DeviceNode::MappingCache::g_cache_lock; + +orb_advert_t uORB::DeviceNode::MappingCache::get(ORB_ID orb_id, uint8_t instance) +{ + lock(); + + MappingCacheListItem *item = g_cache; + + while (item && + (orb_id != node(item->handle)->id() || + instance != node(item->handle)->get_instance())) { + item = item->next; + } + + unlock(); + + return item != nullptr ? item->handle : ORB_ADVERT_INVALID; +} + +orb_advert_t uORB::DeviceNode::MappingCache::map_node(ORB_ID orb_id, uint8_t instance, int shm_fd) +{ + + // Check if it is already mapped + orb_advert_t handle = get(orb_id, instance); + + if (orb_advert_valid(handle)) { + return handle; + } + + lock(); + + // Not mapped yet, map it + void *ptr = px4_mmap(0, sizeof(uORB::DeviceNode), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); + + if (ptr != MAP_FAILED) { + // In NuttX flat and protected builds we can just drop the mappings + // to save some kernel memory. There is no MMU, and the memory is + // there until the shm object is unlinked +#if defined(CONFIG_BUILD_FLAT) + px4_munmap(ptr, sizeof(uORB::DeviceNode)); +#endif + + // Create a list item and add to the beginning of the list + handle.node = ptr; + MappingCacheListItem *item = new MappingCacheListItem{g_cache, handle}; + + if (item) { + g_cache = item; + } + } + + unlock(); + + return handle; +} + +#if !defined(CONFIG_BUILD_FLAT) +orb_advert_t uORB::DeviceNode::MappingCache::map_data(orb_advert_t handle, int shm_fd, size_t size, bool publisher) +{ + lock(); + + MappingCacheListItem *item = g_cache; + + while (item && + handle.node != item->handle.node) { + item = item->next; + } + + if (item != nullptr) { + + if (item->handle.data != nullptr && item->handle.data_size == size) { + // Mapped already, return the mapping + handle = item->handle; + + } else { + // Drop any old mapping if exists + if (handle.data != nullptr) { + px4_munmap(handle.data, handle.data_size); + } + + // Map the data with new size + if (shm_fd >= 0 && size > 0) { + handle.data = px4_mmap(0, size, publisher ? PROT_WRITE : PROT_READ, MAP_SHARED, shm_fd, 0); + + if (handle.data == MAP_FAILED) { + handle.data = nullptr; + handle.data_size = 0; + PX4_ERR("MMAP fail\n"); + + } else { + handle.data_size = size; + } + + } else { + handle.data = nullptr; + handle.data_size = 0; + } + + item->handle = handle; + } + } + + unlock(); + + return handle; +} #endif -static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } +bool uORB::DeviceNode::MappingCache::del(const orb_advert_t &handle) +{ + MappingCacheListItem *prev = nullptr; + + lock(); + + MappingCacheListItem *item = g_cache; + + while (item && + handle.node != item->handle.node) { + prev = item; + item = item->next; + } + + if (item != nullptr) { + if (prev == nullptr) { + // Remove the first item + g_cache = item->next; + + } else { + prev->next = item->next; + } + + px4_munmap(handle.node, sizeof(DeviceNode)); + +#ifndef CONFIG_BUILD_FLAT + + if (handle.data) { + px4_munmap(handle.data, handle.data_size); + } + +#endif + + delete (item); + } + + unlock(); + + return item != nullptr ? true : false; +} // round up to nearest power of two // Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128 @@ -73,210 +242,403 @@ static inline uint8_t round_pow_of_two_8(uint8_t n) return value + 1; } -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, - uint8_t queue_size) : - CDev(strdup(path)), // success is checked in CDev::init - _meta(meta), - _instance(instance), - _queue_size(round_pow_of_two_8(queue_size)) +orb_advert_t uORB::DeviceNode::nodeOpen(const ORB_ID id, const uint8_t instance, bool create) { + /* + * Generate the path to the node and try to open it. + */ + + orb_advert_t handle = MappingCache::get(id, instance); + + if (orb_advert_valid(handle)) { + return handle; + } + + char nodepath[orb_maxpath]; + int inst = instance; + int ret = uORB::Utils::node_mkpath(nodepath, get_orb_meta(id), &inst); + bool created = false; + + if (ret != OK) { + return handle; + } + + // First, try to create the node. This will fail if it already exists + + int shm_fd = -1; + + if (create) { + shm_fd = shm_open(nodepath, O_CREAT | O_RDWR | O_EXCL, 0666); + + if (shm_fd >= 0) { + + // If the creation succeeded, set the size of the shm region + if (ftruncate(shm_fd, sizeof(uORB::DeviceNode)) != 0) { + ::close(shm_fd); + shm_fd = -1; + PX4_ERR("truncate fail!\n"); + + } else { + created = true; + } + } + } + + if (shm_fd < 0) { + // Now try to open an existing one + + shm_fd = shm_open(nodepath, O_RDWR, 0666); + } + + if (shm_fd < 0) { + // We were not able to create a new node or open an existing one + return handle; + } + + handle = MappingCache::map_node(id, instance, shm_fd); + + // No need to keep the fd any more, close it + + ::close(shm_fd); + + if (orb_advert_valid(handle) && created) { + // construct the new node in the region + new (node(handle)) uORB::DeviceNode(id, instance, nodepath); + } + + return handle; } -uORB::DeviceNode::~DeviceNode() +int uORB::DeviceNode::nodeClose(orb_advert_t &handle) { - free(_data); + if (!orb_advert_valid(handle)) { + return PX4_ERROR; + } - const char *devname = get_devname(); + if (node(handle)->_publisher_count == 0) { + node(handle)->_queue_size = 0; + node(handle)->_data_valid = false; - if (devname) { -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) - kmm_free((void *)devname); + // Delete the data +#ifdef CONFIG_BUILD_FLAT + free(node(handle)->_data); + node(handle)->_data = nullptr; #else - free((void *)devname); + shm_unlink(node(handle)->get_devname() + 1); + MappingCache::map_data(handle, -1, 0, false); #endif + + // If there are no more subscribers, delete the node and its mapping + if (node(handle)->_subscriber_count == 0) { + + // Close the Node object + shm_unlink(node(handle)->get_devname()); + + // Uninitialize the node + delete (node(handle)); + + // Delete the mappings for this process + MappingCache::del(handle); + } } + + handle = ORB_ADVERT_INVALID; + + return PX4_OK; } -int -uORB::DeviceNode::open(cdev::file_t *filp) +orb_advert_t uORB::DeviceNode::orb_advertise(const ORB_ID id, int instance, unsigned queue_size, + bool publisher) { - /* is this a publisher? */ - if (filp->f_oflags == PX4_F_WRONLY) { + /* Open the node, if it exists or create a new one */ - lock(); - mark_as_advertised(); - unlock(); + orb_advert_t handle; + handle = nodeOpen(id, instance, true); - /* now complete the open */ - return CDev::open(filp); + if (orb_advert_valid(handle)) { + node(handle)->advertise(publisher, queue_size); } - /* is this a new subscriber? */ - if (filp->f_oflags == PX4_F_RDONLY) { + return handle; +} - /* allocate subscriber data */ - SubscriptionInterval *sd = new SubscriptionInterval(_meta, 0, _instance); +int uORB::DeviceNode::advertise(bool publisher, uint8_t queue_size) +{ + int ret = -1; - if (nullptr == sd) { - return -ENOMEM; - } + ret = ++_advertiser_count; - filp->f_priv = (void *)sd; + if (publisher) { + ret = ++_publisher_count; + } - int ret = CDev::open(filp); + update_queue_size(queue_size); - if (ret != PX4_OK) { - PX4_ERR("CDev::open failed"); - delete sd; - } + return ret; +} - return ret; +int uORB::DeviceNode::orb_unadvertise(orb_advert_t &handle, bool publisher) +{ + int ret = -1; + + if (orb_advert_valid(handle)) { + ret = node(handle)->unadvertise(publisher); + nodeClose(handle); } - if (filp->f_oflags == 0) { - return CDev::open(filp); + return ret; +} + +int uORB::DeviceNode::unadvertise(bool publisher) +{ + int ret = -1; + + ret = --_advertiser_count; + + if (publisher) { + --_publisher_count; } - /* can only be pub or sub, not both */ - return -EINVAL; + return ret; } -int -uORB::DeviceNode::close(cdev::file_t *filp) +uORB::DeviceNode::DeviceNode(const ORB_ID id, const uint8_t instance, const char *path) : + _orb_id(id), + _instance(instance) { - if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */ - SubscriptionInterval *sd = filp_to_subscription(filp); - delete sd; +#if defined(CONFIG_BUILD_FLAT) + _devname = strdup(path); +#else + + if (strnlen(path, sizeof(_devname)) == sizeof(_devname)) { + PX4_ERR("node path too long %s", path); } - return CDev::close(filp); + strncpy(_devname, path, sizeof(_devname)); +#endif + + int ret = px4_sem_init(&_lock, 1, 1); + + if (ret != 0) { + PX4_DEBUG("SEM INIT FAIL: ret %d", ret); + } } -ssize_t -uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) +uORB::DeviceNode::~DeviceNode() { - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) { - return -EIO; + px4_sem_destroy(&_lock); + +#if defined(CONFIG_BUILD_FLAT) + + // Delete all the allocated free callback items. + // There should not be any left in use, since the node is + // deleted only if there are no more publishers or subscribers registered + + IndexedStackHandle callbacks(_callbacks); + uorb_cb_handle_t handle = callbacks.pop_free(); + + while (callbacks.handle_valid(handle)) { + delete (static_cast(callbacks.peek(handle))); + handle = callbacks.pop_free(); } - return filp_to_subscription(filp)->copy(buffer) ? _meta->o_size : 0; + free(_devname); +#endif } -ssize_t -uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) +/* Map the node data to the memory space of publisher or subscriber */ + +void uORB::DeviceNode::remap_data(orb_advert_t &handle, size_t new_size, bool publisher) { - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. + // In NuttX flat and protected builds, just malloc the data (from user heap) + // and store + // the pointer. This saves us the inodes in the + // kernel side. Otherwise the same logic would work + +#ifdef CONFIG_BUILD_FLAT + + // Data size has changed, re-allocate (remap) by publisher + // The remapping may happen only on the first write, + // when the handle.data_size==0 + + if (publisher && handle.data_size == 0) { + free(_data); + _data = malloc(new_size); + } + + if (_data != nullptr) { + handle.data_size = new_size; + + } else { + handle.data_size = 0; + } + +#else + + // Open the data, the data shm name is the same as device node's except for leading '_' + int oflag = publisher ? O_RDWR | O_CREAT : O_RDONLY; + int shm_fd = shm_open(get_devname() + 1, oflag, 0666); + + // and mmap it + if (shm_fd >= 0) { + + // For the publisher, set the new data size + if (publisher && handle.data_size == 0) { + if (ftruncate(shm_fd, new_size) != 0) { + ::close(shm_fd); + PX4_ERR("Setting advertise size failed\n"); + return; + } + } + + handle = MappingCache::map_data(handle, shm_fd, new_size, publisher); + + // Close the shm, there is no need to leave it open + ::close(shm_fd); + } + +#endif +} + +/** + * Copies data and the corresponding generation + * from a node to the buffer provided. * - * Note that filp will usually be NULL. + * @param dst + * The buffer into which the data is copied. + * @param generation + * The generation that was copied. + * @return bool + * Returns true if the data was copied. */ - if (nullptr == _data) { +bool uORB::DeviceNode::copy(void *dst, orb_advert_t &handle, unsigned &generation) +{ + if (dst == nullptr || !_data_valid) { + return false; + } -#ifdef __PX4_NUTTX + size_t o_size = get_meta()->o_size; + size_t data_size = _queue_size * o_size; - if (!up_interrupt_context()) { -#endif /* __PX4_NUTTX */ + ATOMIC_ENTER; - lock(); + if (data_size != handle.data_size) { + remap_data(handle, data_size, false); - /* re-check size */ - if (nullptr == _data) { - const size_t data_size = _meta->o_size * _queue_size; - _data = (uint8_t *) px4_cache_aligned_alloc(data_size); - memset(_data, 0, data_size); - } + if (node_data(handle) == nullptr) { + ATOMIC_LEAVE; + return false; + } + } - unlock(); + if (_queue_size == 1) { + memcpy(dst, node_data(handle), o_size); + generation = _generation.load(); -#ifdef __PX4_NUTTX + } else { + const unsigned current_generation = _generation.load(); + + if (current_generation > generation + _queue_size) { + // Reader is too far behind: some messages are lost + generation = current_generation - _queue_size; } -#endif /* __PX4_NUTTX */ + if ((current_generation == generation) && (generation > 0)) { + /* The subscriber already read the latest message, but nothing new was published yet. + * Return the previous message + */ + --generation; + } - /* failed or could not allocate */ - if (nullptr == _data) { - return -ENOMEM; + memcpy(dst, ((uint8_t *)node_data(handle)) + (o_size * (generation % _queue_size)), o_size); + + if (generation < current_generation) { + ++generation; } } - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) { - return -EIO; - } + ATOMIC_LEAVE; + + return true; + +} + +ssize_t +uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert_t &handle) +{ + size_t o_size = meta->o_size; + + /* If data size has changed, re-map the data */ + size_t data_size = _queue_size * o_size; + + /* Remove single unresponsive entry at a time (if any) */ + uorb_cb_handle_t remove_cb = UORB_INVALID_CB_HANDLE; /* Perform an atomic copy. */ ATOMIC_ENTER; - /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ - unsigned generation = _generation.fetch_add(1); - memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size); + if (data_size != handle.data_size) { + remap_data(handle, data_size, true); + } - // callbacks - for (auto item : _callbacks) { - item->call(); + if (node_data(handle) == nullptr) { + ATOMIC_LEAVE; + return -ENOMEM; } + /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ + unsigned generation = _generation.fetch_add(1); + + memcpy(((uint8_t *)node_data(handle)) + o_size * (generation % _queue_size), buffer, o_size); + /* Mark at least one data has been published */ _data_valid = true; - ATOMIC_LEAVE; + uORB::DeviceNode *n = node(handle); + IndexedStackHandle callbacks(n->_callbacks); - /* notify any poll waiters */ - poll_notify(POLLIN); + uorb_cb_handle_t cb = callbacks.head(); - return _meta->o_size; -} + while (callbacks.handle_valid(cb)) { + EventWaitItem *item = callbacks.peek(cb); -int -uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case ORBIOCUPDATED: { - ATOMIC_ENTER; - *(bool *)arg = filp_to_subscription(filp)->updated(); - ATOMIC_LEAVE; - return PX4_OK; - } - - case ORBIOCSETINTERVAL: - filp_to_subscription(filp)->set_interval_us(arg); - return PX4_OK; + if (item->interval_us == 0 || hrt_elapsed_time(&item->last_update) >= item->interval_us) { + if (item->subscriber != nullptr) { +#ifdef CONFIG_BUILD_FLAT + item->subscriber->call(); +#else + Manager::queueCallback(item->subscriber); +#endif + } - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return PX4_OK; + // Release poll waiters (and callback threads in non-flat builds) + if (item->lock != -1) { + if (Manager::isThreadAlive(item->lock)) { + Manager::unlockThread(item->lock); - case ORBIOCSETQUEUESIZE: { - lock(); - int ret = update_queue_size(arg); - unlock(); - return ret; + } else { + remove_cb = cb; + } + } } - case ORBIOCGETINTERVAL: - *(unsigned *)arg = filp_to_subscription(filp)->get_interval_us(); - return PX4_OK; - - case ORBIOCISADVERTISED: - *(unsigned long *)arg = _advertised; + cb = callbacks.next(cb); + } - return PX4_OK; + ATOMIC_LEAVE; - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + if (callbacks.handle_valid(remove_cb)) { + PX4_ERR("Removing subscriber due to inactivity\n"); + unregister_callback(handle, remove_cb); } + + return o_size; } ssize_t -uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t &handle, const void *data) { - uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; + uORB::DeviceNode *devnode = node(handle); int ret; /* check if the device handle is initialized and data is valid */ @@ -286,18 +648,13 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v } /* check if the orb meta data matches the publication */ - if (devnode->_meta->o_id != meta->o_id) { + if (static_cast(devnode->id()) != meta->o_id) { errno = EINVAL; return PX4_ERROR; } - /* call the devnode write method with no file pointer */ - ret = devnode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) { - errno = -ret; - return PX4_ERROR; - } + /* call the devnode write method */ + ret = devnode->write((const char *)data, meta, handle); if (ret != (int)meta->o_size) { errno = EIO; @@ -322,30 +679,6 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v return PX4_OK; } -int uORB::DeviceNode::unadvertise(orb_advert_t handle) -{ - if (handle == nullptr) { - return -EINVAL; - } - - uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; - - /* - * We are cheating a bit here. First, with the current implementation, we can only - * have multiple publishers for instance 0. In this case the caller will have - * instance=nullptr and _published has no effect at all. Thus no unadvertise is - * necessary. - * In case of multiple instances, we have at most 1 publisher per instance and - * we can signal an instance as 'free' by setting _published to false. - * We never really free the DeviceNode, for this we would need reference counting - * of subscribers and publishers. But we also do not have a leak since future - * publishers reuse the same DeviceNode object. - */ - devnode->_advertised = false; - - return PX4_OK; -} - #ifdef CONFIG_ORB_COMMUNICATOR int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) { @@ -359,28 +692,9 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) } #endif /* CONFIG_ORB_COMMUNICATOR */ -px4_pollevent_t -uORB::DeviceNode::poll_state(cdev::file_t *filp) -{ - // If the topic appears updated to the subscriber, say so. - return filp_to_subscription(filp)->updated() ? POLLIN : 0; -} - -void -uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) -{ - // If the topic looks updated to the subscriber, go ahead and notify them. - if (filp_to_subscription((cdev::file_t *)fds->priv)->updated()) { - CDev::poll_notify_one(fds, events); - } -} - bool uORB::DeviceNode::print_statistics(int max_topic_length) { - if (!_advertised) { - return false; - } lock(); @@ -390,15 +704,40 @@ uORB::DeviceNode::print_statistics(int max_topic_length) unlock(); - PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, - queue_size, get_meta()->o_size, get_devname()); + const orb_metadata *meta = get_meta(); + + PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, meta->o_name, (int)instance, (int)sub_count, + queue_size, meta->o_size, get_devname()); return true; } -void uORB::DeviceNode::add_internal_subscriber() +orb_advert_t uORB::DeviceNode::add_subscriber(ORB_ID orb_id, uint8_t instance, + unsigned *initial_generation, bool advertise) { - lock(); + orb_advert_t handle; + + if (advertise) { + handle = orb_advertise(orb_id, instance, 0, false); + + } else { + handle = nodeOpen(orb_id, instance, false); + } + + if (orb_advert_valid(handle)) { + node(handle)->_add_subscriber(initial_generation); + + } else { + *initial_generation = 0; + } + + return handle; +} + + +void uORB::DeviceNode::_add_subscriber(unsigned *initial_generation) +{ + *initial_generation = _generation.load() - (_data_valid ? 1 : 0); _subscriber_count++; #ifdef CONFIG_ORB_COMMUNICATOR @@ -406,72 +745,84 @@ void uORB::DeviceNode::add_internal_subscriber() if (ch != nullptr && _subscriber_count > 0) { unlock(); //make sure we cannot deadlock if add_subscription calls back into DeviceNode - ch->add_subscription(_meta->o_name, 1); + ch->add_subscription(get_name(), 1); - } else -#endif /* CONFIG_ORB_COMMUNICATOR */ - - { - unlock(); } + +#endif /* CONFIG_ORB_COMMUNICATOR */ } -void uORB::DeviceNode::remove_internal_subscriber() + +int8_t uORB::DeviceNode::remove_subscriber(orb_advert_t &handle, bool advertiser) { - lock(); - _subscriber_count--; + int8_t ret = _subscriber_count--; + + if (advertiser) { + orb_unadvertise(handle, false); + + } else { + nodeClose(handle); + + } #ifdef CONFIG_ORB_COMMUNICATOR uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if (ch != nullptr && _subscriber_count == 0) { - unlock(); //make sure we cannot deadlock if remove_subscription calls back into DeviceNode - ch->remove_subscription(_meta->o_name); + if (ch != nullptr && ret == 0) { + ch->remove_subscription(get_meta()->o_name); - } else -#endif /* CONFIG_ORB_COMMUNICATOR */ - { - unlock(); } + +#endif /* ORB_COMMUNICATOR */ + + return ret; } #ifdef CONFIG_ORB_COMMUNICATOR -int16_t uORB::DeviceNode::process_add_subscription() +int16_t uORB::DeviceNode::process_add_subscription(orb_advert_t &handle) { // if there is already data in the node, send this out to // the remote entity. // send the data to the remote entity. uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + const orb_metadata *meta = get_meta(); - if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. - ch->send_message(_meta->o_name, _meta->o_size, _data); + if (ch != nullptr) { + ch->send_message(meta->o_name, meta->o_size, (uint8_t *)node_data(handle)); } return PX4_OK; } -int16_t uORB::DeviceNode::process_remove_subscription() +int16_t uORB::DeviceNode::process_remove_subscription(orb_advert_t &handle) { return PX4_OK; } -int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) +int16_t uORB::DeviceNode::process_received_message(orb_advert_t &handle, int32_t length, uint8_t *data) { int16_t ret = -1; - if (length != (int32_t)(_meta->o_size)) { - PX4_ERR("Received '%s' with DataLength[%d] != ExpectedLen[%d]", _meta->o_name, (int)length, (int)_meta->o_size); + if (!orb_advert_valid(handle)) { + return ret; + } + + const orb_metadata *meta = get_meta(); + + if (length != (int32_t)(meta->o_size)) { + PX4_ERR("Received '%s' with DataLength[%d] != ExpectedLen[%d]", meta->o_name, (int)length, + (int)meta->o_size); return PX4_ERROR; } - /* call the devnode write method with no file pointer */ - ret = write(nullptr, (const char *)data, _meta->o_size); + /* call the devnode write method */ + ret = write((const char *)data, meta, handle); if (ret < 0) { return PX4_ERROR; } - if (ret != (int)_meta->o_size) { + if (ret != (int)meta->o_size) { errno = EIO; return PX4_ERROR; } @@ -482,57 +833,87 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data int uORB::DeviceNode::update_queue_size(unsigned int queue_size) { - if (_queue_size == queue_size) { + // subscribers may advertise the node, but not set the queue_size + if (queue_size == 0) { return PX4_OK; } - //queue size is limited to 255 for the single reason that we use uint8 to store it - if (_data || _queue_size > queue_size || queue_size > 255) { + queue_size = round_pow_of_two_8(queue_size); + + // queue size is limited to 255 for the single reason that we use uint8 to store it + if (queue_size > 255) { return PX4_ERROR; } - _queue_size = round_pow_of_two_8(queue_size); + _queue_size = queue_size; + return PX4_OK; } -unsigned uORB::DeviceNode::get_initial_generation() +//TODO: make this a normal member function +uorb_cb_handle_t +uORB::DeviceNode::register_callback(orb_advert_t &node_handle, uORB::SubscriptionCallback *callback_sub, + int8_t poll_lock, hrt_abstime last_update, uint32_t interval_us) { - ATOMIC_ENTER; + uORB::DeviceNode *n = node(node_handle); - // If there any previous publications allow the subscriber to read them - unsigned generation = _generation.load() - (_data_valid ? 1 : 0); + n->lock(); - ATOMIC_LEAVE; +#ifndef CONFIG_BUILD_FLAT + // Get the cb lock for this process from the Manager + int8_t lock = poll_lock == -1 ? Manager::getCallbackLock() : poll_lock; +#else + int8_t lock = poll_lock; +#endif - return generation; -} + // TODO: Check for duplicate registrations? -bool -uORB::DeviceNode::register_callback(uORB::SubscriptionCallback *callback_sub) -{ - if (callback_sub != nullptr) { - ATOMIC_ENTER; - - // prevent duplicate registrations - for (auto existing_callbacks : _callbacks) { - if (callback_sub == existing_callbacks) { - ATOMIC_LEAVE; - return true; - } - } + IndexedStackHandle callbacks(n->_callbacks); + uorb_cb_handle_t i = callbacks.pop_free(); + EventWaitItem *item = callbacks.peek(i); - _callbacks.add(callback_sub); - ATOMIC_LEAVE; - return true; +#ifdef CONFIG_BUILD_FLAT + + if (!item) { + item = new EventWaitItem; + i = item; + } + +#endif + + if (item != nullptr) { + item->lock = lock; + item->subscriber = callback_sub; + item->last_update = last_update; + item->interval_us = interval_us; + + callbacks.push(i); + + } else { + PX4_ERR("register fail\n"); } - return false; + n->unlock(); + + return i; } +//TODO: make this a normal member function? void -uORB::DeviceNode::unregister_callback(uORB::SubscriptionCallback *callback_sub) +uORB::DeviceNode::unregister_callback(orb_advert_t &node_handle, uorb_cb_handle_t cb_handle) { - ATOMIC_ENTER; - _callbacks.remove(callback_sub); - ATOMIC_LEAVE; + uORB::DeviceNode *n = node(node_handle); + + n->lock(); + + IndexedStackHandle callbacks(n->_callbacks); + + if (!callbacks.rm(cb_handle)) { + PX4_ERR("unregister fail\n"); + + } else { + callbacks.push_free(cb_handle); + } + + n->unlock(); } diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index c5c5bb22c0c8..3225ab9b023d 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyight (c) 2012-2016 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 @@ -33,37 +33,59 @@ #pragma once -#include "uORBCommon.hpp" -#include "uORBDeviceMaster.hpp" +#include +#include +#include +#include +#include +#include -#include +#include "uORB.h" +#include "uORBCommon.hpp" +#include +#include -#include #include #include #include -namespace uORB -{ -class DeviceNode; -class DeviceMaster; -class Manager; -class SubscriptionCallback; -} +#include "IndexedStack.hpp" + +#if defined(CONFIG_BUILD_FLAT) +#define MAX_EVENT_WAITERS 0 // dynamic +typedef void *uorb_cb_handle_t; +#define UORB_INVALID_CB_HANDLE nullptr +#define uorb_cb_handle_valid(x) ((x) != nullptr) +#else +#define MAX_EVENT_WAITERS 5 +#define UORB_INVALID_CB_HANDLE -1 +typedef int8_t uorb_cb_handle_t; +#define uorb_cb_handle_valid(x) ((x) >= 0) +#endif + +#define CB_LIST_T struct EventWaitItem, uorb_cb_handle_t, MAX_EVENT_WAITERS namespace uORBTest { class UnitTest; } +namespace uORB +{ + +class SubscriptionCallback; + /** * Per-object device instance. */ -class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode +class DeviceNode { public: - DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1); - virtual ~DeviceNode(); + // Open a node, either existing or create a new one + static orb_advert_t nodeOpen(const ORB_ID id, const uint8_t instance, bool create); + static int nodeClose(orb_advert_t &handle); + + ~DeviceNode(); // no copy, assignment, move, move assignment DeviceNode(const DeviceNode &) = delete; @@ -74,56 +96,25 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeget_name(); - static int unadvertise(orb_advert_t handle); + } else { + return nullptr; + } + } -#ifdef CONFIG_ORB_COMMUNICATOR /** * processes a request for topic advertisement from remote * @param meta @@ -140,53 +131,40 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode(_meta->o_id); } + ORB_ID id() const { return _orb_id; } - const char *get_name() const { return _meta->o_name; } + const char *get_name() const { return get_orb_meta(_orb_id)->o_name; } uint8_t get_instance() const { return _instance; } @@ -231,82 +212,133 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeo_size); - generation = _generation.load(); - ATOMIC_LEAVE; - return true; - - } else { - ATOMIC_ENTER; - const unsigned current_generation = _generation.load(); - - if (current_generation == generation) { - /* The subscriber already read the latest message, but nothing new was published yet. - * Return the previous message - */ - --generation; - } - - // Compatible with normal and overflow conditions - if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) { - // Reader is too far behind: some messages are lost - generation = current_generation - _queue_size; - } - - memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size); - ATOMIC_LEAVE; - - ++generation; - - return true; - } - } - - return false; + bool copy(void *dst, orb_advert_t &handle, unsigned &generation); - } + static void orb_callback(int signo, siginfo_t *si_info, void *data); - // add item to list of work items to schedule on node update - bool register_callback(SubscriptionCallback *callback_sub); + static uorb_cb_handle_t register_callback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, + int8_t poll_lock, + hrt_abstime last_update, uint32_t interval_us); - // remove item from list of work items - void unregister_callback(SubscriptionCallback *callback_sub); + static void unregister_callback(orb_advert_t &node_handle, uorb_cb_handle_t cb_handle); -protected: + void *operator new (size_t, void *p) + { + return p; + } - px4_pollevent_t poll_state(cdev::file_t *filp) override; + void operator delete (void *p) + { + } - void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) override; + const char *get_devname() const {return _devname;} private: friend uORBTest::UnitTest; - const orb_metadata *_meta; /**< object metadata information */ + //const orb_metadata *_meta; /**< object metadata information */ + const ORB_ID _orb_id; - uint8_t *_data{nullptr}; /**< allocated object buffer */ bool _data_valid{false}; /**< At least one valid data */ px4::atomic _generation{0}; /**< object generation count */ - List _callbacks; - const uint8_t _instance; /**< orb multi instance identifier */ - bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ - uint8_t _queue_size; /**< maximum number of elements in the queue */ - int8_t _subscriber_count{0}; + struct EventWaitItem { + struct SubscriptionCallback *subscriber; + hrt_abstime last_update; + uint32_t interval_us; + int8_t lock; + uorb_cb_handle_t next; // List ptr + }; + IndexedStack _callbacks; -// Determine the data range - static inline bool is_in_range(unsigned left, unsigned value, unsigned right) - { - if (right > left) { - return (left <= value) && (value <= right); + inline ssize_t write(const char *buffer, const orb_metadata *meta, orb_advert_t &handle); + static int callback_thread(int argc, char *argv[]); + struct SubscriptionCallback *callback_sub; - } else { // Maybe the data overflowed and a wraparound occurred - return (left <= value) || (value <= right); + class MappingCache + { + public: + struct MappingCacheListItem { + MappingCacheListItem *next; + orb_advert_t handle; + }; + + // This list is process specific in kernel build and global in in flat + static MappingCacheListItem *g_cache; + static px4_sem_t g_cache_lock; + + static void init(void) + { + static bool initialized = false; + + if (!initialized) { + px4_sem_init(&g_cache_lock, 0, 1); + initialized = true; + } } - } + static orb_advert_t get(ORB_ID orb_id, uint8_t instance); + static orb_advert_t map_node(ORB_ID orb_id, uint8_t instance, int shm_fd); + static orb_advert_t map_data(orb_advert_t handle, int shm_fd, size_t size, bool publisher); + static bool del(const orb_advert_t &handle); + + static void lock() + { + init(); + do {} while (px4_sem_wait(&g_cache_lock) != 0); + } + static void unlock() { px4_sem_post(&g_cache_lock); } + }; + + const uint8_t _instance; /**< orb multi instance identifier */ + uint8_t _queue_size{0}; /**< maximum number of elements in the queue */ + int8_t _subscriber_count{0}; /**< how many subscriptions there are */ + int8_t _publisher_count{0}; /**< how many publishers have advertised this topic */ + int8_t _advertiser_count{0}; /**< how many total advertisers this topic has */ + + DeviceNode(const ORB_ID id, const uint8_t instance, const char *path); + + int advertise(bool publisher, uint8_t queue_size); + int unadvertise(bool publisher); + + /** + * Change the size of the queue. + * @return PX4_OK if queue size successfully set + */ + int update_queue_size(unsigned int queue_size); + + void _add_subscriber(unsigned *initial_generation); + + /** + * Each device node instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + * + * Careful: lock() calls cannot be nested! + */ + void lock() { do {} while (px4_sem_wait(&_lock) != 0); } + + /** + * Release the device node lock. + */ + void unlock() { px4_sem_post(&_lock); } + + void remap_data(orb_advert_t &handle, size_t new_size, bool advertiser); + + inline static DeviceNode *node(const orb_advert_t &handle) { return static_cast(handle.node); } +#ifdef CONFIG_BUILD_FLAT + inline static void *node_data(const orb_advert_t &handle) { return node(handle)->_data; } +#else + inline static void *node_data(const orb_advert_t &handle) { return handle.data; } +#endif + + px4_sem_t _lock; /**< lock to protect access to all class members */ + +#ifdef CONFIG_BUILD_FLAT + char *_devname; + void *_data{nullptr}; +#else + char _devname[NAME_MAX + 1]; +#endif }; +} //namespace uORB diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 5b5530694b94..fa0fa6919166 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -31,22 +31,28 @@ * ****************************************************************************/ +#include #include #include +#include #include #include +#include #include #include #include -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) -#include -#endif - #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" +#include "SubscriptionCallback.hpp" + +#ifndef CONFIG_FS_SHMFS_VFS_PATH +#define CONFIG_FS_SHMFS_VFS_PATH "/dev/shm" +#endif + +static const char uORBManagerName[] = "_uORB_Manager"; #ifdef CONFIG_ORB_COMMUNICATOR pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER; @@ -54,23 +60,95 @@ pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER; uORB::Manager *uORB::Manager::_Instance = nullptr; +// This is the per-process lock for callback thread +#ifndef CONFIG_BUILD_FLAT +int8_t uORB::Manager::per_process_lock = -1; +pid_t uORB::Manager::per_process_cb_thread = -1; +#endif + +void uORB::Manager::cleanup() +{ + // TODO: This is operating system dependent. Works on linux and NuttX + DIR *shm_dir = opendir(CONFIG_FS_SHMFS_VFS_PATH); + struct dirent *next_file; + + // Delete all uorb shm allocations + while ((next_file = readdir(shm_dir)) != nullptr) { + // build the path for each file in the folder + if (!strncmp(next_file->d_name, "orb_", 4) || + !strncmp(next_file->d_name, "_orb_", 5)) { + shm_unlink(next_file->d_name); + } + } + + closedir(shm_dir); + + // Delete manager shm allocations + shm_unlink(uORBManagerName); +} + bool uORB::Manager::initialize() { if (_Instance == nullptr) { - _Instance = new uORB::Manager(); + + // Cleanup from previous execution, in case some shm files are left + cleanup(); + + // Create a shared memory segment for uORB Manager and initialize a new manager into it + int shm_fd = shm_open(uORBManagerName, O_CREAT | O_RDWR, 0666); + + if (shm_fd >= 0) { + // If the creation succeeded, set the size + if (ftruncate(shm_fd, sizeof(uORB::Manager)) == 0) { + // mmap the shared memory region + void *ptr = px4_mmap(0, sizeof(uORB::Manager), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); + _Instance = new (ptr) uORB::Manager(); + + for (auto &publisher : _Instance->g_has_publisher) { + publisher = 0; + } + } + } } -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) +#if defined(CONFIG_FS_SHMFS_WRPROTECT) px4_register_boardct_ioctl(_ORBIOCDEVBASE, orb_ioctl); #endif + return _Instance != nullptr; } +void uORB::Manager::map_instance() +{ + if (_Instance == nullptr) { + + // Open the existing manager + int shm_fd = shm_open(uORBManagerName, O_RDWR, 0666); + + if (shm_fd >= 0) { + // mmap the shared memory region + void *ptr = px4_mmap(0, sizeof(uORB::Manager), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); + + if (ptr != MAP_FAILED) { + _Instance = (uORB::Manager *)ptr; + } + } + } + + if (_Instance == nullptr) { + PX4_ERR("FATAL: Can't get uORB manager"); + } +} + bool uORB::Manager::terminate() { + // We don't delete or unmap the Manager. Cleanup will + // unlink the SHM, and all the mappings are dropped when the + // processes exit + + cleanup(); + if (_Instance != nullptr) { - delete _Instance; - _Instance = nullptr; return true; } @@ -79,9 +157,11 @@ bool uORB::Manager::terminate() uORB::Manager::Manager() { + int ret; + #ifdef ORB_USE_PUBLISHER_RULES const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules"; - int ret = readPublisherRulesFromFile(file_name, _publisher_rule); + ret = readPublisherRulesFromFile(file_name, _publisher_rule); if (ret == PX4_OK) { _has_publisher_rules = true; @@ -93,180 +173,46 @@ uORB::Manager::Manager() #endif /* ORB_USE_PUBLISHER_RULES */ -} + ret = px4_sem_init(&_lock, 1, 1); -uORB::Manager::~Manager() -{ - delete _device_master; -} + if (ret != 0) { + PX4_DEBUG("SEM INIT FAIL: ret %d", ret); + } -uORB::DeviceMaster *uORB::Manager::get_device_master() -{ - if (!_device_master) { - _device_master = new DeviceMaster(); + ret = px4_sem_init(&_callback_lock, 1, 1); - if (_device_master == nullptr) { - PX4_ERR("Failed to allocate DeviceMaster"); - errno = ENOMEM; - } + if (ret != 0) { + PX4_DEBUG("SEM INIT FAIL: ret %d", ret); } - return _device_master; + g_sem_pool.init(); } -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) -int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) +uORB::Manager::~Manager() { - int ret = PX4_OK; - - switch (cmd) { - case ORBIOCDEVEXISTS: { - orbiocdevexists_t *data = (orbiocdevexists_t *)arg; - - if (data->check_advertised) { - if (uORB::Manager::get_instance()) { - data->ret = uORB::Manager::get_instance()->orb_exists(get_orb_meta(data->orb_id), data->instance); - - } else { - data->ret = PX4_ERROR; - } - - } else { - data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR; - } - } - break; - - case ORBIOCDEVADVERTISE: { - orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg; - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); - - if (dev) { - data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance); - - } else { - data->ret = PX4_ERROR; - } - } - break; - - case ORBIOCDEVUNADVERTISE: { - orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg; - data->ret = uORB::Manager::orb_unadvertise(data->handle); - } - break; - - case ORBIOCDEVPUBLISH: { - orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg; - data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data); - } - break; - - case ORBIOCDEVADDSUBSCRIBER: { - orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg; - data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation); - } - break; - - case ORBIOCDEVREMSUBSCRIBER: { - uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast(arg)); - } - break; - - case ORBIOCDEVQUEUESIZE: { - orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg; - data->size = uORB::Manager::orb_get_queue_size(data->handle); - } - break; - - case ORBIOCDEVDATACOPY: { - orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg; - data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated); - } - break; - - case ORBIOCDEVREGCALLBACK: { - orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg; - data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub); - } - break; - - case ORBIOCDEVUNREGCALLBACK: { - orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg; - uORB::Manager::unregister_callback(data->handle, data->callback_sub); - } - break; - - case ORBIOCDEVGETINSTANCE: { - orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg; - data->instance = uORB::Manager::orb_get_instance(data->handle); - } - break; - - case ORBIOCDEVMASTERCMD: { - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); - - if (dev) { - if (arg == ORB_DEVMASTER_TOP) { - dev->showTop(nullptr, 0); - - } else { - dev->printStatistics(); - } - } - } - break; - - case ORBIOCDEVUPDATESAVAIL: { - orbiocdevupdatesavail_t *data = (orbiocdevupdatesavail_t *)arg; - data->ret = updates_available(data->handle, data->last_generation); - } - break; - - case ORBIOCDEVISADVERTISED: { - orbiocdevisadvertised_t *data = (orbiocdevisadvertised_t *)arg; - data->ret = is_advertised(data->handle); - } - break; - - default: - ret = -ENOTTY; - } - - return ret; + px4_sem_destroy(&_lock); + px4_sem_destroy(&_callback_lock); } -#endif int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { - if (meta == nullptr) { - return PX4_ERROR; - } - - int ret = PX4_ERROR; - // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) - if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { - return ret; + if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1)) || meta == nullptr) { + return PX4_ERROR; } - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); - - if (dev) { - uORB::DeviceNode *node = dev->getDeviceNode(meta, instance); - - if (node != nullptr) { - if (node->is_advertised()) { - return PX4_OK; - } - } + // meta != nullptr + // orb is advertised by a publisher + if (meta != nullptr && + has_publisher(static_cast(meta->o_id), instance)) { + return PX4_OK; } - return ret; + return PX4_ERROR; } orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) + uint8_t queue_size) { #ifdef ORB_USE_PUBLISHER_RULES @@ -278,124 +224,130 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, if (_publisher_rule.ignore_other_topics) { if (!findTopic(_publisher_rule, meta->o_name)) { PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name); - return (orb_advert_t)_Instance; + return ORB_ADVERT_INVALID; } } } else { if (findTopic(_publisher_rule, meta->o_name)) { PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name); - return (orb_advert_t)_Instance; + return ORB_ADVERT_INVALID; } } } #endif /* ORB_USE_PUBLISHER_RULES */ - /* open the node as an advertiser */ - int fd = node_open(meta, true, instance); + // Calculate the wanted instance + unsigned group_tries = 0; - if (fd == PX4_ERROR) { - PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); - return nullptr; - } + lock(); + + while (group_tries < ORB_MULTI_MAX_INSTANCES) { - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); + // is not advertised by a publisher or is a single instance publisher + if (!has_publisher(static_cast(meta->o_id), group_tries) || !instance) { + break; + } - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); + group_tries++; } - /* get the advertiser handle and close the node */ - orb_advert_t advertiser; + if (group_tries == ORB_MULTI_MAX_INSTANCES) { + unlock(); + PX4_ERR("%s: too many instances (%d)", meta->o_name, group_tries); + return ORB_ADVERT_INVALID; + } - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); + orb_advert_t handle = uORB::DeviceNode::orb_advertise(static_cast(meta->o_id), group_tries, queue_size, true); - if (result == PX4_ERROR) { - PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); - return nullptr; + if (instance != nullptr) { + *instance = group_tries; } -#ifdef CONFIG_ORB_COMMUNICATOR + // Cache existence of this node instance globally + if (orb_advert_valid(handle)) { + set_has_publisher(static_cast(meta->o_id), group_tries); - // Advertise to the remote side, but only if it is a local topic. Otherwise - // we will generate an advertisement loop. - if (_remote_topics.find(meta->o_name) == false) { +#ifdef CONFIG_ORB_COMMUNICATOR + // For remote systems call over and inform them uORB::DeviceNode::topic_advertised(meta); +#endif /* CONFIG_ORB_COMMUNICATOR */ + + } else { + PX4_ERR("orb_advertise_multi failed %s", meta->o_name); } -#endif /* CONFIG_ORB_COMMUNICATOR */ + unlock(); /* the advertiser may perform an initial publish to initialise the object */ - if (data != nullptr) { - result = orb_publish(meta, advertiser, data); + + if (data != nullptr && orb_advert_valid(handle)) { + int result = orb_publish(meta, handle, data); if (result == PX4_ERROR) { PX4_ERR("orb_publish failed %s", meta->o_name); - return nullptr; + orb_unadvertise(handle); } } - return advertiser; + return handle; } -int uORB::Manager::orb_unadvertise(orb_advert_t handle) +int uORB::Manager::orb_unadvertise(orb_advert_t &handle) { -#ifdef ORB_USE_PUBLISHER_RULES - - if (handle == _Instance) { - return PX4_OK; //pretend success + if (!orb_advert_valid(handle)) { + return PX4_ERROR; } -#endif /* ORB_USE_PUBLISHER_RULES */ + ORB_ID id = static_cast(node(handle)->id()); + uint8_t instance = node(handle)->get_instance(); - return uORB::DeviceNode::unadvertise(handle); -} + Manager *manager = get_instance(); -int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(meta, false); -} + manager->lock(); -int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) -{ - int inst = instance; - return node_open(meta, false, &inst); + bool unadvertised = uORB::DeviceNode::orb_unadvertise(handle, true) >= 0; + + // Node is deleted and handle invalidated, if the last advertiser goes away + + if (!orb_advert_valid(handle) || node(handle)->publisher_count() == 0) { + manager->unset_has_publisher(id, instance); + } + + manager->unlock(); + + return unadvertised ? PX4_OK : PX4_ERROR; } -int uORB::Manager::orb_unsubscribe(int fd) +// Should only be called from old interface +orb_sub_t uORB::Manager::orb_subscribe(const struct orb_metadata *meta) { - return px4_close(fd); + return orb_subscribe_multi(meta, 0); } -int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +// Should only be called from old interface +orb_sub_t uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { -#ifdef ORB_USE_PUBLISHER_RULES + uORB::SubscriptionInterval *sub = new uORB::SubscriptionPollable(meta, instance); - if (handle == _Instance) { - return PX4_OK; //pretend success + if (sub && !sub->valid()) { + // subscribe and advertise the topic + sub->subscribe(true); } -#endif /* ORB_USE_PUBLISHER_RULES */ - - return uORB::DeviceNode::publish(meta, handle, data); + return sub; } -int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +int uORB::Manager::orb_unsubscribe(orb_sub_t handle) { - int ret; - - ret = px4_read(handle, buffer, meta->o_size); - - if (ret < 0) { - return PX4_ERROR; - } + delete (static_cast(handle)); + return PX4_OK; +} - if (ret != (int)meta->o_size) { +int uORB::Manager::orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer) +{ + if (!(static_cast(handle))->copy(buffer)) { errno = EIO; return PX4_ERROR; } @@ -403,190 +355,218 @@ int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *b return PX4_OK; } -int uORB::Manager::orb_check(int handle, bool *updated) +int uORB::Manager::orb_check(orb_sub_t handle, bool *updated) { - /* Set to false here so that if `px4_ioctl` fails to false. */ - *updated = false; - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + *updated = ((uORB::SubscriptionInterval *)handle)->updated(); + return PX4_OK; } -int uORB::Manager::orb_set_interval(int handle, unsigned interval) +int uORB::Manager::orb_set_interval(orb_sub_t handle, unsigned interval) { - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + ((uORB::SubscriptionInterval *)handle)->set_interval_us(interval * 1000); + return PX4_OK; } -int uORB::Manager::orb_get_interval(int handle, unsigned *interval) +int uORB::Manager::orb_get_interval(orb_sub_t handle, unsigned *interval) { - int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); - *interval /= 1000; - return ret; + *interval = ((uORB::SubscriptionInterval *)handle)->get_interval_us() / 1000; + return PX4_OK; } - -bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout) { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + SubscriptionPollable *sub; - return device_master != nullptr && - device_master->deviceNodeExists(orb_id, instance); -} + // Get a poll semaphore from the global pool + int8_t lock_idx = g_sem_pool.reserve(); -void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) -{ - uORB::DeviceNode *node = nullptr; - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + if (lock_idx < 0) { + PX4_ERR("Out of thread locks"); + return -1; + } + + // Any orb updated already? + bool err = false; + int count = 0; - if (device_master != nullptr) { - node = device_master->getDeviceNode(get_orb_meta(orb_id), instance); + for (unsigned i = 0; i < nfds; i++) { + fds[i].revents = 0; - if (node) { - node->add_internal_subscriber(); - *initial_generation = node->get_initial_generation(); + if ((fds[i].events & POLLIN) == POLLIN) { + sub = static_cast(fds[i].fd); + sub->registerPoll(lock_idx); + + if (sub->updated()) { + fds[i].revents = POLLIN; + count++; + } } } - return node; -} + // If none of the orbs were updated before registration, go to sleep. + // If some orb was updated after registration, but not yet refelected in "updated", the semaphore is already released. So there is no race in here. -void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) -{ - static_cast(node_handle)->remove_internal_subscriber(); -} + if (count == 0) { -uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast(node_handle)->get_queue_size(); } + // First advertiser will wake us up, or it might have happened already + // during registration above -bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) -{ - if (!is_advertised(node_handle)) { - return false; + int ret; + + if (timeout < 0) { + // Wait event until interrupted by a signal + ret = g_sem_pool.take_interruptible(lock_idx); + + } else { + // Wait event for a maximum timeout time + struct timespec to; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + px4_clock_gettime(CLOCK_MONOTONIC, &to); +#else + px4_clock_gettime(CLOCK_REALTIME, &to); +#endif + hrt_abstime now = ts_to_abstime(&to); + abstime_to_ts(&to, now + (hrt_abstime)timeout * 1000); + ret = g_sem_pool.take_timedwait(lock_idx, &to); + } + + if (ret != 0 && errno != ETIMEDOUT && errno != EINTR) { + PX4_ERR("poll on %d timeout %d FAIL errno %d\n", lock_idx, timeout, errno); + err = true; + } } - if (only_if_updated && !static_cast(node_handle)->updates_available(generation)) { - return false; + count = 0; + + for (unsigned i = 0; i < nfds; i++) { + if ((fds[i].events & POLLIN) == POLLIN) { + sub = static_cast(fds[i].fd); + sub->unregisterPoll(); + + if (sub->updated()) { + fds[i].revents |= POLLIN; + count++; + } + } } - return static_cast(node_handle)->copy(dst, generation); -} + // recover from releasing multiple times + g_sem_pool.set(lock_idx, 0); + g_sem_pool.free(lock_idx); -// add item to list of work items to schedule on node update -bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - return static_cast(node_handle)->register_callback(callback_sub); + return err ? -1 : count; } -// remove item from list of work items -void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - static_cast(node_handle)->unregister_callback(callback_sub); -} +#ifndef CONFIG_BUILD_FLAT -uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +int8_t +uORB::Manager::launchCallbackThread() { - if (node_handle) { - return static_cast(node_handle)->get_instance(); + per_process_lock = Manager::getThreadLock(); + + if (per_process_lock < 0) { + PX4_ERR("Out of thread locks\n"); + return -1; } - return -1; -} + if (per_process_cb_thread == -1) { + per_process_cb_thread = px4_task_spawn_cmd("orb_callback", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 1, + PX4_STACK_ADJUSTED(1024), + callback_thread, + nullptr); -/* These are optimized by inlining in NuttX Flat build */ -#if !defined(CONFIG_BUILD_FLAT) -unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) -{ - return is_advertised(node_handle) ? static_cast(node_handle)->updates_available( - last_generation) : 0; -} + if (per_process_cb_thread < 0) { + PX4_ERR("callback thread creation failed\n"); + Manager::freeThreadLock(per_process_lock); + return -1; + } + } -bool uORB::Manager::is_advertised(const void *node_handle) -{ - return static_cast(node_handle)->is_advertised(); + return per_process_lock; } -#endif -int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) +int +uORB::Manager::callback_thread(int argc, char *argv[]) { - char path[orb_maxpath]; - int fd = -1; - int ret = PX4_ERROR; + while (true) { + lockThread(per_process_lock); - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return PX4_ERROR; + SubscriptionCallback *sub = _Instance->_callback_ptr; + _Instance->unlock_callbacks(); + + // Pass nullptr to this thread to exit + if (sub == nullptr) { + break; + } + + sub->call(); } - /* if we have an instance and are an advertiser, we will generate a new node and set the instance, - * so we do not need to open here */ - if (!instance || !advertiser) { - /* - * Generate the path to the node and try to open it. - */ - ret = uORB::Utils::node_mkpath(path, meta, instance); + Manager::freeThreadLock(per_process_lock); + per_process_lock = -1; - if (ret != OK) { - errno = -ret; - return PX4_ERROR; - } + return 0; +} + +#endif - /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); - } else { - *instance = 0; +void uORB::Manager::GlobalSemPool::init(void) +{ + for (auto &sem : _global_sem) { + sem.init(); } - /* we may need to advertise the node... */ - if (fd < 0) { + px4_sem_init(&_semLock, 1, 1); +} - ret = PX4_ERROR; +void uORB::Manager::GlobalSemPool::free(int8_t i) +{ + lock(); - if (get_device_master()) { - ret = _device_master->advertise(meta, advertiser, instance); - } + _global_sem[i].in_use = false; - /* it's OK if it already exists */ - if ((ret != PX4_OK) && (EEXIST == errno)) { - ret = PX4_OK; - } + unlock(); +} - if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node advertise call */ - ret = uORB::Utils::node_mkpath(path, meta, instance); +int8_t uORB::Manager::GlobalSemPool::reserve() +{ + lock(); - /* on success, try to open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + // Find the first free lock + int8_t i; - } else { - errno = -ret; - return PX4_ERROR; - } + for (i = 0; i < NUM_GLOBAL_SEMS; i++) { + if (!_global_sem[i].in_use) { + break; } } - if (fd < 0) { - errno = EIO; - return PX4_ERROR; + // Check that we got one + if (i == NUM_GLOBAL_SEMS) { + PX4_ERR("Out of global locks"); + unlock(); + return -1; } - /* everything has been OK, we can return the handle now */ - return fd; + // Mark this one as in use + _global_sem[i].in_use = true; + + unlock(); + + return i; } #ifdef CONFIG_ORB_COMMUNICATOR void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel) { - pthread_mutex_lock(&_communicator_mutex); + _comm_channel = channel; - if (channel != nullptr) { - channel->register_handler(this); - _comm_channel = channel; + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); } - - pthread_mutex_unlock(&_communicator_mutex); } uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator() @@ -598,30 +578,8 @@ uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator() return temp; } -int16_t uORB::Manager::process_remote_topic(const char *topic_name) +const struct orb_metadata *uORB::Manager::topic_name_to_meta(const char *topic_name) { - PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); - - // Look to see if we already have a node for this topic - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, topic_name); - - if (ret == OK) { - DeviceMaster *device_master = get_device_master(); - - if (device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); - - if (node) { - PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name); - node->mark_as_advertised(); - _remote_topics.insert(topic_name); - return 0; - } - } - } - - // We didn't find a node so we need to create it via an advertisement const struct orb_metadata *const *topic_list = orb_get_topics(); orb_id_t topic_ptr = nullptr; @@ -632,43 +590,63 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) } } - if (topic_ptr) { - PX4_INFO("Advertising remote topic %s", topic_name); - _remote_topics.insert(topic_name); - // Add some queue depth when advertising remote topics. These - // topics may get aggregated and thus delivered in a batch that - // requires some buffering in a queue. - orb_advertise(topic_ptr, nullptr, 5); + return topic_ptr; +} - } else { +int16_t uORB::Manager::process_remote_topic(const char *topic_name) +{ + PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); + + int16_t rc = 0; + + const struct orb_metadata *meta = topic_name_to_meta(topic_name); + + if (meta == nullptr) { PX4_INFO("process_remote_topic meta not found for %s\n", topic_name); + _remote_topics.erase(topic_name); + return -1; } - return 0; + _Instance->lock(); + + orb_advert_t handle = orb_advertise(meta, nullptr); + + _Instance->unlock(); + + if (orb_advert_valid(handle)) { + PX4_INFO("Advertising remote publisher %s", topic_name); + _remote_topics.insert(handle); + + } else { + PX4_INFO("Advertisement failed"); + rc = -1; + } + + return rc; } int16_t uORB::Manager::process_add_subscription(const char *messageName) { PX4_DEBUG("entering Manager_process_add_subscription: name: %s", messageName); - int16_t rc = 0; - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, messageName); - DeviceMaster *device_master = get_device_master(); + int16_t rc = -1; - if (ret == OK && device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + const struct orb_metadata *meta = topic_name_to_meta(messageName); + + if (meta == nullptr) { + return rc; + } - if (node == nullptr) { - PX4_DEBUG("DeviceNode(%s) not created yet", messageName); + orb_advert_t handle = DeviceNode::nodeOpen(static_cast(meta->o_id), 0, false); - } else { - // node is present. - node->process_add_subscription(); - } + if (!orb_advert_valid(handle)) { + PX4_DEBUG("DeviceNode(%s) not created yet", messageName); } else { - rc = -1; + // node is present. + + node(handle)->process_add_subscription(handle); + rc = 0; } return rc; @@ -677,23 +655,24 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName) int16_t uORB::Manager::process_remove_subscription(const char *messageName) { int16_t rc = -1; - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, messageName); - DeviceMaster *device_master = get_device_master(); - if (ret == OK && device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + const struct orb_metadata *meta = topic_name_to_meta(messageName); - // get the node name. - if (node == nullptr) { - PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName); + if (meta == nullptr) { + PX4_DEBUG("DeviceNode(%s) meta not found", messageName); + return rc; + } - } else { - // node is present. - node->process_remove_subscription(); - rc = 0; - } + orb_advert_t handle = DeviceNode::nodeOpen(static_cast(meta->o_id), 0, false); + + if (!orb_advert_valid(handle)) { + PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); + + } else { + // node is present. + node(handle)->process_remove_subscription(handle); + rc = 0; } return rc; @@ -702,22 +681,16 @@ int16_t uORB::Manager::process_remove_subscription(const char *messageName) int16_t uORB::Manager::process_received_message(const char *messageName, int32_t length, uint8_t *data) { int16_t rc = -1; - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, messageName); - DeviceMaster *device_master = get_device_master(); - if (ret == OK && device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + orb_advert_t handle = _remote_topics.find(messageName); - // get the node name. - if (node == nullptr) { - PX4_DEBUG("No existing subscriber found for message: [%s] nodepath:[%s]", messageName, nodepath); + if (!orb_advert_valid(handle)) { + PX4_DEBUG("No existing subscriber found for message: [%s]", messageName); - } else { - // node is present. - node->process_received_message(length, data); - rc = 0; - } + } else { + // node is present. + node(handle)->process_received_message(handle, length, data); + rc = 0; } return rc; @@ -862,4 +835,5 @@ int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRu fclose(fp); return ret; } + #endif /* ORB_USE_PUBLISHER_RULES */ diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 1c2870b1eecd..28ed486854f3 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -31,12 +31,14 @@ * ****************************************************************************/ -#ifndef _uORBManager_hpp_ -#define _uORBManager_hpp_ +#pragma once + +#include +#include +#include #include "uORBDeviceNode.hpp" #include "uORBCommon.hpp" -#include "uORBDeviceMaster.hpp" #include // For ORB_ID enum #include @@ -47,119 +49,18 @@ #include "uORBCommunicator.hpp" #endif /* CONFIG_ORB_COMMUNICATOR */ +#define NUM_GLOBAL_SEMS 40 +#define SUB_ALIVE_SEM_MAX_VALUE 100 + namespace uORB { -class Manager; -class SubscriptionCallback; -} - - -/* - * IOCTLs for manager to access device nodes using - * a handle - */ - -#define _ORBIOCDEV(_n) (_PX4_IOC(_ORBIOCDEVBASE, _n)) -#define ORBIOCDEVEXISTS _ORBIOCDEV(30) -typedef struct orbiocdevexists { - const ORB_ID orb_id; - const uint8_t instance; - const bool check_advertised; - int ret; -} orbiocdevexists_t; - -#define ORBIOCDEVADVERTISE _ORBIOCDEV(31) -typedef struct orbiocadvertise { - const struct orb_metadata *meta; - bool is_advertiser; - int *instance; - int ret; -} orbiocdevadvertise_t; - -#define ORBIOCDEVUNADVERTISE _ORBIOCDEV(32) -typedef struct orbiocunadvertise { - void *handle; - int ret; -} orbiocdevunadvertise_t; - -#define ORBIOCDEVPUBLISH _ORBIOCDEV(33) -typedef struct orbiocpublish { - const struct orb_metadata *meta; - orb_advert_t handle; - const void *data; - int ret; -} orbiocdevpublish_t; - -#define ORBIOCDEVADDSUBSCRIBER _ORBIOCDEV(34) -typedef struct { - const ORB_ID orb_id; - const uint8_t instance; - unsigned *initial_generation; - void *handle; -} orbiocdevaddsubscriber_t; - -#define ORBIOCDEVREMSUBSCRIBER _ORBIOCDEV(35) - -#define ORBIOCDEVQUEUESIZE _ORBIOCDEV(36) -typedef struct { - const void *handle; - uint8_t size; -} orbiocdevqueuesize_t; - -#define ORBIOCDEVDATACOPY _ORBIOCDEV(37) -typedef struct { - void *handle; - void *dst; - unsigned generation; - bool only_if_updated; - bool ret; -} orbiocdevdatacopy_t; - -#define ORBIOCDEVREGCALLBACK _ORBIOCDEV(38) -typedef struct { - void *handle; - class uORB::SubscriptionCallback *callback_sub; - bool registered; -} orbiocdevregcallback_t; - -#define ORBIOCDEVUNREGCALLBACK _ORBIOCDEV(39) -typedef struct { - void *handle; - class uORB::SubscriptionCallback *callback_sub; -} orbiocdevunregcallback_t; - -#define ORBIOCDEVGETINSTANCE _ORBIOCDEV(40) -typedef struct { - const void *handle; - uint8_t instance; -} orbiocdevgetinstance_t; - -#define ORBIOCDEVUPDATESAVAIL _ORBIOCDEV(41) -typedef struct { - const void *handle; - unsigned last_generation; - unsigned ret; -} orbiocdevupdatesavail_t; - -#define ORBIOCDEVISADVERTISED _ORBIOCDEV(42) -typedef struct { - const void *handle; - bool ret; -} orbiocdevisadvertised_t; - -typedef enum { - ORB_DEVMASTER_STATUS = 0, - ORB_DEVMASTER_TOP = 1 -} orbiocdevmastercmd_t; -#define ORBIOCDEVMASTERCMD _ORBIOCDEV(45) - /** * This is implemented as a singleton. This class manages creating the * uORB nodes for each uORB topics and also implements the behavor of the * uORB Api's. */ -class uORB::Manager +class Manager #ifdef CONFIG_ORB_COMMUNICATOR : public uORBCommunicator::IChannelRxHandler #endif /* CONFIG_ORB_COMMUNICATOR */ @@ -181,22 +82,16 @@ class uORB::Manager /** * Method to get the singleton instance for the uORB::Manager. - * Make sure initialize() is called first. * @return uORB::Manager* */ - static uORB::Manager *get_instance() { return _Instance; } - - /** - * Get the DeviceMaster. If it does not exist, - * it will be created and initialized. - * Note: the first call to this is not thread-safe. - * @return nullptr if initialization failed (and errno will be set) - */ - uORB::DeviceMaster *get_device_master(); + static uORB::Manager *get_instance() + { + if (_Instance == nullptr) { + map_instance(); + } -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) - static int orb_ioctl(unsigned int cmd, unsigned long arg); -#endif + return _Instance; + } // ==== uORB interface methods ==== /** @@ -259,7 +154,7 @@ class uORB::Manager * this function will return nullptr and set errno to ENOENT. */ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size = 1); + uint8_t queue_size = 1); /** * Unadvertise a topic. @@ -267,7 +162,7 @@ class uORB::Manager * @param handle handle returned by orb_advertise or orb_advertise_multi. * @return 0 on success */ - static int orb_unadvertise(orb_advert_t handle); + static int orb_unadvertise(orb_advert_t &handle); /** * Publish new data to a topic. @@ -282,12 +177,12 @@ class uORB::Manager * @param data A pointer to the data to be published. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - static int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data); + static int orb_publish(const struct orb_metadata *meta, orb_advert_t &handle, const void *data) {return uORB::DeviceNode::publish(meta, handle, data);} /** * Subscribe to a topic. * - * The returned value is a file descriptor that can be passed to poll() + * The returned value is a handle that can be passed to orb_poll() * in order to wait for updates to a topic, as well as topic_read, * orb_check. * @@ -312,12 +207,12 @@ class uORB::Manager * @return PX4_ERROR on error, otherwise returns a handle * that can be used to read and update the topic. */ - int orb_subscribe(const struct orb_metadata *meta); + orb_sub_t orb_subscribe(const struct orb_metadata *meta); /** * Subscribe to a multi-instance of a topic. * - * The returned value is a file descriptor that can be passed to poll() + * The returned value is a handle that can be passed to orb_poll() * in order to wait for updates to a topic, as well as topic_read, * orb_check. * @@ -344,13 +239,13 @@ class uORB::Manager * @param instance The instance of the topic. Instance 0 matches the * topic of the orb_subscribe() call, higher indices * are for topics created with orb_advertise_multi(). - * @return PX4_ERROR on error, otherwise returns a handle + * @return returns a handle * that can be used to read and update the topic. * If the topic in question is not known (due to an * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. + * this function will return an invalid handle and set errno to ENOENT. */ - int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance); + orb_sub_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance); /** * Unsubscribe from a topic. @@ -358,14 +253,14 @@ class uORB::Manager * @param handle A handle returned from orb_subscribe. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_unsubscribe(int handle); + static int orb_unsubscribe(orb_sub_t handle); /** * Fetch data from a topic. * * This is the only operation that will reset the internal marker that * indicates that a topic has been updated for a subscriber. Once poll - * or check return indicating that an updaet is available, this call + * or check return indicating that an update is available, this call * must be used to update the subscription. * * @param meta The uORB metadata (usually from the ORB_ID() macro) @@ -376,7 +271,7 @@ class uORB::Manager * using the data. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_copy(const struct orb_metadata *meta, int handle, void *buffer); + static int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer); /** * Check whether a topic has been published to since the last orb_copy. @@ -394,7 +289,7 @@ class uORB::Manager * @return OK if the check was successful, PX4_ERROR otherwise with * errno set accordingly. */ - int orb_check(int handle, bool *updated); + static int orb_check(orb_sub_t handle, bool *updated); /** * Check if a topic has already been created and published (advertised) @@ -403,7 +298,7 @@ class uORB::Manager * @param instance ORB instance * @return OK if the topic exists, PX4_ERROR otherwise. */ - int orb_exists(const struct orb_metadata *meta, int instance); + static int orb_exists(const struct orb_metadata *meta, int instance); /** * Set the minimum interval between which updates are seen for a subscription. @@ -423,8 +318,7 @@ class uORB::Manager * @param interval An interval period in milliseconds. * @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly. */ - int orb_set_interval(int handle, unsigned interval); - + static int orb_set_interval(orb_sub_t handle, unsigned interval); /** * Get the minimum interval between which updates are seen for a subscription. @@ -435,37 +329,80 @@ class uORB::Manager * @param interval The returned interval period in milliseconds. * @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly. */ - int orb_get_interval(int handle, unsigned *interval); + static int orb_get_interval(orb_sub_t handle, unsigned *interval); - static bool orb_device_node_exists(ORB_ID orb_id, uint8_t instance); + int orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout); - static void *orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation); - - static void orb_remove_internal_subscriber(void *node_handle); + static orb_advert_t orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation, + bool advertise) + { + if (!advertise && !has_publisher(orb_id, instance)) { + return ORB_ADVERT_INVALID; + } - static uint8_t orb_get_queue_size(const void *node_handle); + _Instance->lock(); + orb_advert_t handle = uORB::DeviceNode::add_subscriber(orb_id, instance, initial_generation, advertise); + _Instance->unlock(); - static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated); + return handle; + } - static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub); + static void orb_remove_internal_subscriber(orb_advert_t &node_handle, bool advertiser) + { + _Instance->lock(); + node(node_handle)->remove_subscriber(node_handle, advertiser); + _Instance->unlock(); + } - static void unregister_callback(void *node_handle, SubscriptionCallback *callback_sub); + static uint8_t orb_get_queue_size(const orb_advert_t &node_handle) {return node(node_handle)->get_queue_size();} - static uint8_t orb_get_instance(const void *node_handle); + static bool orb_data_copy(orb_advert_t &node_handle, void *dst, unsigned &generation, + bool only_if_updated) + { + if (!orb_advert_valid(node_handle) || + (only_if_updated && !node(node_handle)->updates_available(generation))) { + return false; + } -#if defined(CONFIG_BUILD_FLAT) - /* These are optimized by inlining in NuttX Flat build */ - static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast(node_handle)->updates_available(last_generation) : 0; } + return node(node_handle)->copy(dst, node_handle, generation); + } - static bool is_advertised(const void *node_handle) { return static_cast(node_handle)->is_advertised(); } +#ifndef CONFIG_BUILD_FLAT + static uint8_t getCallbackLock() + { + uint8_t cbLock; -#else - static unsigned updates_available(const void *node_handle, unsigned last_generation); + // TODO: think about if this needs protection, maybe not use the + // same lock as for node advertise/subscribe - static bool is_advertised(const void *node_handle); + _Instance->lock(); + cbLock = per_process_lock >= 0 ? per_process_lock : launchCallbackThread(); + _Instance->unlock(); + return cbLock; + } #endif + static uint8_t orb_get_instance(orb_advert_t &node_handle) + { + if (orb_advert_valid(node_handle)) { + return node(node_handle)->get_instance(); + } + + return -1; + } + + static unsigned updates_available(const orb_advert_t &node_handle, unsigned last_generation) + { + return node(node_handle)->updates_available(last_generation); + } + + static bool has_publisher(ORB_ID orb_id, uint8_t instance) + { + return (get_instance()->g_has_publisher[static_cast(orb_id)] & (1 << instance)) != 0; + } + #ifdef CONFIG_ORB_COMMUNICATOR + /** * Method to set the uORBCommunicator::IChannel instance. * @param comm_channel @@ -483,15 +420,49 @@ class uORB::Manager #endif /* CONFIG_ORB_COMMUNICATOR */ + void *operator new (size_t, void *p) + { + return p; + } + + void operator delete (void *p) + { + px4_munmap(p, sizeof(uORB::Manager)); + } + + static void lockThread(int idx) + { + _Instance->g_sem_pool.take(idx); + } + + static void unlockThread(int idx) + { + _Instance->g_sem_pool.release(idx); + } + + static void freeThreadLock(int i) {_Instance->g_sem_pool.free(i);} + + static int8_t getThreadLock() {return _Instance->g_sem_pool.reserve();} + + static void queueCallback(class SubscriptionCallback *sub) + { + _Instance->lock_callbacks(); + _Instance->_callback_ptr = sub; + // The manager is unlocked in callback thread + } + + static bool isThreadAlive(int idx) + { + int value = _Instance->g_sem_pool.value(idx); + return value <= SUB_ALIVE_SEM_MAX_VALUE; + } + private: // class methods + inline static uORB::DeviceNode *node(orb_advert_t handle) {return static_cast(handle.node);} - /** - * Common implementation for orb_advertise and orb_subscribe. - * - * Handles creation of the object and the initial publication for - * advertisers. - */ - int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr); + static void cleanup(); + static int callback_thread(int argc, char *argv[]); + static int8_t launchCallbackThread(); private: // data members static Manager *_Instance; @@ -505,13 +476,34 @@ class uORB::Manager ORBSet _remote_topics; #endif /* CONFIG_ORB_COMMUNICATOR */ - DeviceMaster *_device_master{nullptr}; - private: //class methods Manager(); - virtual ~Manager(); + ~Manager(); + + /** + * Lock against node concurrent node creation + */ + void lock() { do {} while (px4_sem_wait(&_lock) != 0); } + + /** + * Release the node creation lock + */ + void unlock() { px4_sem_post(&_lock); } + + px4_sem_t _lock; #ifdef CONFIG_ORB_COMMUNICATOR + /** + * Helper function to find orb_metadata struct by topic name + * @param topic_name + * This represents the uORB message Name (topic); This message Name should be + * globally unique. + * @return + * pointer to struct orb_metadata + * nullptr = failure + */ + const struct orb_metadata *topic_name_to_meta(const char *topic_name); + /** * Interface to process a received topic advertisement from remote. * @param topic_name @@ -598,12 +590,105 @@ class uORB::Manager * @return 0 on success, <0 otherwise */ int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule); - PublisherRule _publisher_rule; bool _has_publisher_rules = false; #endif /* ORB_USE_PUBLISHER_RULES */ -}; + /** + * Method to map the singleton instance for the uORB::Manager + * to the processes address space. Make sure initialize() is called first. + */ + static void map_instance(); + + void set_has_publisher(ORB_ID orb_id, uint8_t instance) + { + static_assert(sizeof(g_has_publisher[0]) * 8 >= ORB_MULTI_MAX_INSTANCES); + g_has_publisher[static_cast(orb_id)] |= (1 << instance); + } -#endif /* _uORBManager_hpp_ */ + void unset_has_publisher(ORB_ID orb_id, uint8_t instance) + { + static_assert(sizeof(g_has_publisher[0]) * 8 >= ORB_MULTI_MAX_INSTANCES); + g_has_publisher[static_cast(orb_id)] &= ~(1 << instance); + } + + // Global cache for advertised uORB node instances + uint16_t g_has_publisher[ORB_TOPICS_COUNT + 1]; + + // This (system global) variable is used to pass the subsriber + // pointer to the callback thread. This is in Manager, since + // it needs to be mapped for both advertisers and the subscribers + class SubscriptionCallback *_callback_ptr {nullptr}; + + // This mutex protects the above pointer for one writer at a time + px4_sem_t _callback_lock; + + void lock_callbacks() { do {} while (px4_sem_wait(&_callback_lock) != 0); } + void unlock_callbacks() { px4_sem_post(&_callback_lock); } + + // A global pool of semaphores for + // 1) poll locks + // 2) callback thread signalling (except in NuttX flat build) + + class GlobalSemPool + { + public: + void init(); + int8_t reserve(); + void free(int8_t i); + + void set(int8_t i, int val) {_global_sem[i].set(val);} + void take(int8_t i) { do {} while (_global_sem[i].take() != 0); } + int take_interruptible(int8_t i) { return _global_sem[i].take(); } + int take_timedwait(int8_t i, struct timespec *abstime) { return _global_sem[i].take_timedwait(abstime); } + void release(int8_t i) {_global_sem[i].release(); } + int value(int8_t i) { return _global_sem[i].value(); } + + class GlobalLock + { + public: + void init() + { + px4_sem_init(&_sem, 1, 0); +#if __PX4_NUTTX + sem_setprotocol(&_sem, SEM_PRIO_NONE); +#endif + in_use = false; + } + void set(int val) + { + px4_sem_destroy(&_sem); + px4_sem_init(&_sem, 1, val); +#if __PX4_NUTTX + sem_setprotocol(&_sem, SEM_PRIO_NONE); +#endif + } + int take() {return px4_sem_wait(&_sem);} + int take_timedwait(struct timespec *abstime) { return px4_sem_timedwait(&_sem, abstime); } + void release() {px4_sem_post(&_sem); } + int value() { int value; px4_sem_getvalue(&_sem, &value); return value; } + bool in_use{false}; + + private: + struct SubscriptionCallback *subscriber; + px4_sem_t _sem; + }; + private: + + void lock() + { + do {} while (px4_sem_wait(&_semLock) != 0); + } + void unlock() { px4_sem_post(&_semLock); } + + GlobalLock _global_sem[NUM_GLOBAL_SEMS]; + px4_sem_t _semLock; + } g_sem_pool; + +#ifndef CONFIG_BUILD_FLAT + static int8_t per_process_lock; + static pid_t per_process_cb_thread; +#endif +}; +} // namespace uORB diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp deleted file mode 100644 index af00dbb9ba58..000000000000 --- a/platforms/common/uORB/uORBManagerUsr.cpp +++ /dev/null @@ -1,349 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 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. - * - ****************************************************************************/ - -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include "uORBDeviceNode.hpp" -#include "uORBUtils.hpp" -#include "uORBManager.hpp" - -uORB::Manager *uORB::Manager::_Instance = nullptr; - -bool uORB::Manager::initialize() -{ - if (_Instance == nullptr) { - _Instance = new uORB::Manager(); - } - - return _Instance != nullptr; -} - -bool uORB::Manager::terminate() -{ - if (_Instance != nullptr) { - delete _Instance; - _Instance = nullptr; - return true; - } - - return false; -} - -uORB::Manager::Manager() -{ -} - -uORB::Manager::~Manager() -{ -} - -int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) -{ - // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) - if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { - return PX4_ERROR; - } - - orbiocdevexists_t data = {static_cast(meta->o_id), static_cast(instance), true, PX4_ERROR}; - boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); - - return data.ret; -} - -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) -{ - /* open the node as an advertiser */ - int fd = node_open(meta, true, instance); - - if (fd == PX4_ERROR) { - PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); - return nullptr; - } - - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); - - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); - } - - /* get the advertiser handle and close the node */ - orb_advert_t advertiser; - - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); - - if (result == PX4_ERROR) { - PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); - return nullptr; - } - - /* the advertiser may perform an initial publish to initialise the object */ - if (data != nullptr) { - result = orb_publish(meta, advertiser, data); - - if (result == PX4_ERROR) { - PX4_ERR("orb_publish failed %s", meta->o_name); - return nullptr; - } - } - - return advertiser; -} - -int uORB::Manager::orb_unadvertise(orb_advert_t handle) -{ - orbiocdevunadvertise_t data = {handle, PX4_ERROR}; - boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast(&data)); - - return data.ret; -} - -int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(meta, false); -} - -int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) -{ - int inst = instance; - return node_open(meta, false, &inst); -} - -int uORB::Manager::orb_unsubscribe(int fd) -{ - return px4_close(fd); -} - -int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) -{ - orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR}; - boardctl(ORBIOCDEVPUBLISH, reinterpret_cast(&d)); - - return d.ret; -} - -int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) -{ - int ret; - - ret = px4_read(handle, buffer, meta->o_size); - - if (ret < 0) { - return PX4_ERROR; - } - - if (ret != (int)meta->o_size) { - errno = EIO; - return PX4_ERROR; - } - - return PX4_OK; -} - -int uORB::Manager::orb_check(int handle, bool *updated) -{ - /* Set to false here so that if `px4_ioctl` fails to false. */ - *updated = false; - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); -} - -int uORB::Manager::orb_set_interval(int handle, unsigned interval) -{ - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); -} - -int uORB::Manager::orb_get_interval(int handle, unsigned *interval) -{ - int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); - *interval /= 1000; - return ret; -} - -int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) -{ - char path[orb_maxpath]; - int fd = -1; - int ret = PX4_ERROR; - - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return PX4_ERROR; - } - - /* if we have an instance and are an advertiser, we will generate a new node and set the instance, - * so we do not need to open here */ - if (!instance || !advertiser) { - /* - * Generate the path to the node and try to open it. - */ - ret = uORB::Utils::node_mkpath(path, meta, instance); - - if (ret != OK) { - errno = -ret; - return PX4_ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); - - } else { - *instance = 0; - } - - /* we may need to advertise the node... */ - if (fd < 0) { - ret = PX4_ERROR; - - orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR}; - boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data); - ret = data.ret; - - /* it's OK if it already exists */ - if ((ret != PX4_OK) && (EEXIST == errno)) { - ret = PX4_OK; - } - - if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node advertise call */ - ret = uORB::Utils::node_mkpath(path, meta, instance); - - /* on success, try to open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - - } else { - errno = -ret; - return PX4_ERROR; - } - } - } - - if (fd < 0) { - errno = EIO; - return PX4_ERROR; - } - - /* everything has been OK, we can return the handle now */ - return fd; -} - -bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) -{ - orbiocdevexists_t data = {orb_id, instance, false, 0}; - boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); - - return data.ret == PX4_OK ? true : false; -} - -void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) -{ - orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr}; - boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast(&data)); - - return data.handle; -} - -void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) -{ - boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast(node_handle)); -} - -uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) -{ - orbiocdevqueuesize_t data = {node_handle, 0}; - boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast(&data)); - - return data.size; -} - -bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) -{ - orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false}; - boardctl(ORBIOCDEVDATACOPY, reinterpret_cast(&data)); - generation = data.generation; - - return data.ret; -} - -bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - orbiocdevregcallback_t data = {node_handle, callback_sub, false}; - boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast(&data)); - - return data.registered; -} - -void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - orbiocdevunregcallback_t data = {node_handle, callback_sub}; - boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast(&data)); -} - -uint8_t uORB::Manager::orb_get_instance(const void *node_handle) -{ - orbiocdevgetinstance_t data = {node_handle, 0}; - boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast(&data)); - - return data.instance; -} - -unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) -{ - orbiocdevupdatesavail_t data = {node_handle, last_generation, 0}; - boardctl(ORBIOCDEVUPDATESAVAIL, reinterpret_cast(&data)); - return data.ret; -} - -bool uORB::Manager::is_advertised(const void *node_handle) -{ - orbiocdevisadvertised_t data = {node_handle, false}; - boardctl(ORBIOCDEVISADVERTISED, reinterpret_cast(&data)); - return data.ret; -} diff --git a/platforms/common/uORB/uORBUtils.cpp b/platforms/common/uORB/uORBUtils.cpp index fc9a88095f2d..c37dcc0873ef 100644 --- a/platforms/common/uORB/uORBUtils.cpp +++ b/platforms/common/uORB/uORBUtils.cpp @@ -45,7 +45,7 @@ int uORB::Utils::node_mkpath(char *buf, const struct orb_metadata *meta, int *in index = *instance; } - len = snprintf(buf, orb_maxpath, "/%s/%s%d", "obj", meta->o_name, index); + len = snprintf(buf, orb_maxpath, "_orb_%s%d", meta->o_name, index); if (len >= orb_maxpath) { return -ENAMETOOLONG; @@ -62,7 +62,7 @@ int uORB::Utils::node_mkpath(char *buf, const char *orbMsgName) unsigned index = 0; - len = snprintf(buf, orb_maxpath, "/%s/%s%d", "obj", orbMsgName, index); + len = snprintf(buf, orb_maxpath, "_orb_%s%d", orbMsgName, index); if (len >= orb_maxpath) { return -ENAMETOOLONG; diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index bc7cda513297..d9f2e7cd8776 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -56,7 +56,7 @@ int uORBTest::UnitTest::pubsublatency_main() /* wakeup source(s) */ px4_pollfd_struct_t fds[1] {}; - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + orb_sub_t test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); orb_test_medium_s t{}; @@ -258,13 +258,13 @@ int uORBTest::UnitTest::test_single() t.val = 0; ptopic = orb_advertise(ORB_ID(orb_test), &t); - if (ptopic == nullptr) { + if (!orb_advert_valid(ptopic)) { return test_fail("advertise failed: %d", errno); } - int sfd = orb_subscribe(ORB_ID(orb_test)); + orb_sub_t sfd = orb_subscribe(ORB_ID(orb_test)); - if (sfd < 0) { + if (!orb_sub_valid(sfd)) { return test_fail("subscribe failed: %d", errno); } @@ -289,7 +289,7 @@ int uORBTest::UnitTest::test_single() t.val = 2; - if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_test), &ptopic, &t)) { return test_fail("publish failed"); } @@ -344,18 +344,18 @@ int uORBTest::UnitTest::test_multi() t.val = 103; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[0], &t)) { return test_fail("mult. pub0 fail"); } t.val = 203; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[1], &t)) { return test_fail("mult. pub1 fail"); } /* subscribe to both topics and ensure valid data is received */ - int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + orb_sub_t sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) { return test_fail("sub #0 copy failed: %d", errno); @@ -365,7 +365,7 @@ int uORBTest::UnitTest::test_multi() return test_fail("sub #0 val. mismatch: %d", u.val); } - int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + orb_sub_t sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) { return test_fail("sub #1 copy failed: %d", errno); @@ -422,7 +422,7 @@ int uORBTest::UnitTest::pub_test_multi2_main() data_topic.timestamp = hrt_absolute_time(); data_topic.val = data_next_idx; - orb_publish(ORB_ID(orb_test_medium_multi), orb_pub[data_next_idx], &data_topic); + orb_publish(ORB_ID(orb_test_medium_multi), &orb_pub[data_next_idx], &data_topic); // PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time); data_next_idx = (data_next_idx + 1) % num_instances; @@ -449,7 +449,7 @@ int uORBTest::UnitTest::test_multi2() _thread_should_exit = false; const int num_instances = 3; - int orb_data_fd[num_instances] {-1, -1, -1}; + orb_sub_t orb_data_fd[num_instances] {ORB_SUB_INVALID, ORB_SUB_INVALID, ORB_SUB_INVALID}; int orb_data_next = 0; for (int i = 0; i < num_instances; ++i) { @@ -476,7 +476,7 @@ int uORBTest::UnitTest::test_multi2() px4_usleep(1000); bool updated = false; - int orb_data_cur_fd = orb_data_fd[orb_data_next]; + orb_sub_t orb_data_cur_fd = orb_data_fd[orb_data_next]; orb_check(orb_data_cur_fd, &updated); if (updated) { @@ -508,9 +508,9 @@ int uORBTest::UnitTest::test_multi_reversed() /* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */ /* Subscribe first and advertise afterwards. */ - int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); + orb_sub_t sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); - if (sfd2 < 0) { + if (!orb_sub_valid(sfd2)) { return test_fail("sub. id2: ret: %d", sfd2); } @@ -533,13 +533,13 @@ int uORBTest::UnitTest::test_multi_reversed() t.val = 204; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[2], &t)) { return test_fail("mult. pub0 fail"); } t.val = 304; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[3], &t)) { return test_fail("mult. pub1 fail"); } @@ -551,7 +551,7 @@ int uORBTest::UnitTest::test_multi_reversed() return test_fail("sub #3 val. mismatch: %d", u.val); } - int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); + orb_sub_t sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) { return test_fail("sub #3 copy failed: %d", errno); @@ -570,20 +570,21 @@ int uORBTest::UnitTest::test_wrap_around() orb_test_medium_s t{}; orb_test_medium_s u{}; - orb_advert_t ptopic{nullptr}; + orb_advert_t ptopic{ORB_ADVERT_INVALID}; bool updated{false}; // Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation const int queue_size = 16; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size); - if (ptopic == nullptr) { + if (!orb_advert_valid(ptopic)) { return test_fail("advertise failed: %d", errno); } - auto node = uORB::Manager::get_instance()->get_device_master()->getDeviceNode(ORB_ID(orb_test_medium_wrap_around), 0); + orb_advert_t handle = uORB::DeviceNode::nodeOpen(ORB_ID::orb_test_medium_wrap_around, 0, false); + auto node = static_cast(handle.node); - if (node == nullptr) { + if (!orb_advert_valid(handle)) { return test_fail("get device node failed."); } @@ -598,11 +599,11 @@ int uORBTest::UnitTest::test_wrap_around() } t.val = 0; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); - int sfd = orb_subscribe(ORB_ID(orb_test_medium_wrap_around)); + orb_sub_t sfd = orb_subscribe(ORB_ID(orb_test_medium_wrap_around)); - if (sfd < 0) { + if (!orb_sub_valid(sfd)) { return test_fail("subscribe failed: %d", errno); } @@ -648,7 +649,7 @@ int uORBTest::UnitTest::test_wrap_around() for (int i = 0; i < queue_size - 2; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); } for (int i = 0; i < queue_size - 2; ++i) { @@ -676,7 +677,7 @@ int uORBTest::UnitTest::test_wrap_around() for (int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); } for (int i = 0; i < queue_size; ++i) { @@ -691,7 +692,7 @@ int uORBTest::UnitTest::test_wrap_around() SET_GENERTATION(); t.val = queue_size; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); @@ -704,7 +705,7 @@ int uORBTest::UnitTest::test_wrap_around() #undef SET_GENERTATION t.val = 943; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); @@ -822,9 +823,9 @@ int uORBTest::UnitTest::test_queue() orb_advert_t ptopic{nullptr}; bool updated{false}; - int sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); + orb_sub_t sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); - if (sfd < 0) { + if (!orb_sub_valid(sfd)) { return test_fail("subscribe failed: %d", errno); } @@ -832,7 +833,7 @@ int uORBTest::UnitTest::test_queue() orb_test_medium_s t{}; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); - if (ptopic == nullptr) { + if (!orb_advert_valid(ptopic)) { return test_fail("advertise failed: %d", errno); } @@ -878,7 +879,7 @@ int uORBTest::UnitTest::test_queue() for (int i = 0; i < queue_size - 2; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue), &ptopic, &t); } for (int i = 0; i < queue_size - 2; ++i) { @@ -893,7 +894,7 @@ int uORBTest::UnitTest::test_queue() for (int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue), &ptopic, &t); } for (int i = 0; i < queue_size; ++i) { @@ -911,7 +912,7 @@ int uORBTest::UnitTest::test_queue() } t.val = 943; - orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue), &ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); @@ -934,10 +935,10 @@ int uORBTest::UnitTest::pub_test_queue_entry(int argc, char *argv[]) int uORBTest::UnitTest::pub_test_queue_main() { orb_test_medium_s t{}; - orb_advert_t ptopic{nullptr}; + orb_advert_t ptopic{ORB_ADVERT_INVALID}; const int queue_size = 50; - if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { + if (!orb_advert_valid(ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size))) { _thread_should_exit = true; return test_fail("advertise failed: %d", errno); } @@ -953,7 +954,7 @@ int uORBTest::UnitTest::pub_test_queue_main() int burst_counter = 0; while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned - orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue_poll), &ptopic, &t); ++t.val; } @@ -974,9 +975,9 @@ int uORBTest::UnitTest::test_queue_poll_notify() test_note("Testing orb queuing (poll & notify)"); orb_test_medium_s t{}; - int sfd = -1; + orb_sub_t sfd = ORB_SUB_INVALID; - if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) { + if (!orb_sub_valid(sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll)))) { return test_fail("subscribe failed: %d", errno); } @@ -1042,7 +1043,7 @@ int uORBTest::UnitTest::latency_test(bool print) orb_advert_t pfd0 = orb_advertise(ORB_ID(orb_test_medium), &t); - if (pfd0 == nullptr) { + if (!orb_advert_valid(pfd0)) { return test_fail("orb_advertise failed (%i)", errno); } @@ -1068,7 +1069,7 @@ int uORBTest::UnitTest::latency_test(bool print) ++t.val; t.timestamp = hrt_absolute_time(); - if (PX4_OK != orb_publish(ORB_ID(orb_test_medium), pfd0, &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_test_medium), &pfd0, &t)) { return test_fail("mult. pub0 timing fail"); } diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp index 908eeca9c7e2..7f93d9f8a732 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -35,7 +35,6 @@ #define _uORBTest_UnitTest_hpp_ #include -#include #include #include #include diff --git a/platforms/common/work_queue/hrt_thread.c b/platforms/common/work_queue/hrt_thread.c index 9465f0d9f21b..cd414fad30af 100644 --- a/platforms/common/work_queue/hrt_thread.c +++ b/platforms/common/work_queue/hrt_thread.c @@ -286,11 +286,16 @@ void hrt_work_queue_init(void) work_hrtthread, (char *const *)NULL); + struct sigaction act; + memset(&act, 0, sizeof(struct sigaction)); + sigemptyset(&act.sa_mask); + act.sa_handler = _sighandler; #ifdef __PX4_QURT - signal(SIGALRM, _sighandler); + sigaddset(&act.sa_mask, SIGALRM); + sigaction(SIGALRM, &act, NULL); #else - signal(SIGCONT, _sighandler); + sigaddset(&act.sa_mask, SIGCONT); + sigaction(SIGCONT, &act, NULL); #endif } - diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 02556996e0e7..0544d01a179d 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -38,7 +38,15 @@ endif() include(cygwin_cygpath) add_executable(px4 ${PX4_SOURCE_DIR}/platforms/common/empty.c) -set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) +if (CONFIG_OPENSBI AND "${PX4_BOARD_LABEL}" STREQUAL "bootloader") + set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}_int.elf) + set(FW_NAME_FLASH ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) + set(FW_NAME_SBI ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_sbi.bin) + set(RISCV_RELAXATIONS -Wl,--relax) +else() + set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) + set(RISCV_RELAXATIONS) +endif() set_target_properties(px4 PROPERTIES OUTPUT_NAME ${FW_NAME}) add_dependencies(px4 git_nuttx) @@ -98,6 +106,17 @@ else() endif() endif() +# Select the correct linker script(s) for build type +if (CONFIG_BUILD_FLAT) + set(LDSCRIPT ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}script.ld) +elseif (CONFIG_BUILD_PROTECTED) + set(LDMEMORY ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld) + set(LDSCRIPT ${LDMEMORY},--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}user-space.ld) + set(LDKERNEL ${LDMEMORY},--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel-space.ld) +else() + message(FATAL_ERROR "Cannot determine linker script for build type") +endif() + list(APPEND nuttx_libs nuttx_boards nuttx_drivers @@ -128,6 +147,12 @@ endif() if(CONFIG_NET) list(APPEND nuttx_libs nuttx_net) target_link_libraries(nuttx_fs INTERFACE nuttx_net) + target_link_libraries(nuttx_drivers INTERFACE nuttx_net) +endif() + +if(CONFIG_OPENAMP) + list(APPEND nuttx_libs nuttx_openamp) + target_link_libraries(nuttx_drivers INTERFACE nuttx_openamp) endif() file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_DIR}) @@ -136,12 +161,16 @@ file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_D # because even relative linker script paths are different for linux, mac and windows CYGPATH(NUTTX_CONFIG_DIR NUTTX_CONFIG_DIR_CYG) -if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external")) +if((DEFINED ENV{SIGNING_TOOL}) AND (NOT "${PX4_BOARD_LABEL}" STREQUAL "bootloader") AND (NOT NUTTX_DIR MATCHES "external")) set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR}/${PX4_CONFIG}_unsigned.bin) + add_subdirectory(toc) + add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_CONFIG}.bin - COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin - DEPENDS ${PX4_BINARY_OUTPUT} + COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_DIR}/toc.bin ${PX4_BINARY_DIR}/toc_signed.bin + COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_CONFIG}_signed.bin + COMMAND cat ${PX4_BINARY_DIR}/toc_signed.bin ${PX4_BINARY_DIR}/${PX4_CONFIG}_signed.bin > ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + DEPENDS toc_bin ${PX4_BINARY_OUTPUT} WORKING_DIRECTORY ${PX4_SOURCE_DIR} ) else() @@ -166,7 +195,7 @@ if (NOT CONFIG_BUILD_FLAT) -fno-exceptions -fno-rtti - -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel-space.ld + -Wl,--script=${LDKERNEL} -Wl,-Map=${PX4_CONFIG}_kernel.map -Wl,--warn-common @@ -212,7 +241,7 @@ if (NOT CONFIG_BUILD_FLAT) -fno-exceptions -fno-rtti - -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}user-space.ld + -Wl,--script=${LDSCRIPT} -Wl,-Map=${PX4_CONFIG}.map -Wl,--warn-common @@ -242,7 +271,7 @@ if (NOT CONFIG_BUILD_FLAT) else() - target_link_libraries(nuttx_c INTERFACE nuttx_sched) # nxsched_get_streams + target_link_libraries(nuttx_c INTERFACE nuttx_sched) target_link_libraries(nuttx_arch INTERFACE @@ -253,9 +282,9 @@ else() target_link_libraries(nuttx_c INTERFACE nuttx_drivers) target_link_libraries(nuttx_drivers INTERFACE nuttx_c) + target_link_libraries(nuttx_xx INTERFACE nuttx_c) target_link_libraries(nuttx_fs INTERFACE nuttx_c) - target_link_libraries(px4 PRIVATE -nostartfiles @@ -265,7 +294,8 @@ else() -fno-exceptions -fno-rtti - -Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld + ${RISCV_RELAXATIONS} + -Wl,--script=${LDSCRIPT} -Wl,-Map=${PX4_CONFIG}.map -Wl,--warn-common -Wl,--gc-sections @@ -289,13 +319,24 @@ else() target_link_libraries(px4 PRIVATE romfs) endif() - add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} - COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} - DEPENDS px4 - ) + # for opensbi bootloader, strip the l2lim & ddr sections + if (CONFIG_OPENSBI AND "${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -R .l2_scratchpad ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_DIR_REL}/${FW_NAME_FLASH} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME_FLASH} ${PX4_BINARY_OUTPUT} + DEPENDS px4 + ) + else() + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} + DEPENDS px4 + ) + endif() endif() +add_custom_target(px4_binary DEPENDS ${PX4_BINARY_OUTPUT}) + # create .px4 with parameter and airframe metadata if (TARGET parameters_xml AND TARGET airframes_xml) @@ -328,7 +369,7 @@ if (TARGET parameters_xml AND TARGET airframes_xml) endif() -if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader" AND NOT CONFIG_OPENSBI) # TODO: handle opensbi add_custom_command(OUTPUT ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin DEPENDS px4 diff --git a/platforms/nuttx/NuttX/CMakeLists.txt b/platforms/nuttx/NuttX/CMakeLists.txt index 355abf0b23f8..168e5acb6b7d 100644 --- a/platforms/nuttx/NuttX/CMakeLists.txt +++ b/platforms/nuttx/NuttX/CMakeLists.txt @@ -212,6 +212,10 @@ if(CONFIG_NET) add_nuttx_dir(net net y -D__KERNEL__ all) endif() +if(CONFIG_OPENAMP) + add_nuttx_dir(openamp openamp y -D__KERNEL__ all) +endif() + ############################################################################### # NuttX oldconfig: Update a configuration using a provided .config as base add_custom_target(oldconfig_nuttx diff --git a/platforms/nuttx/NuttX/Make.defs.in b/platforms/nuttx/NuttX/Make.defs.in index 315bf92300ec..d6fc6c5f152a 100644 --- a/platforms/nuttx/NuttX/Make.defs.in +++ b/platforms/nuttx/NuttX/Make.defs.in @@ -62,7 +62,7 @@ CXXINCPATH := $(shell $(INCDIR) -s "$(CC)" $(TOPDIR)$(DELIM)include$(DELIM)cxx) ARCHINCLUDES += $(CINCPATH) ARCHXXINCLUDES += $(CINCPATH) $(CXXINCPATH) -ARCHSCRIPT = -T$(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld +ARCHSCRIPT = $(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld ifeq ($(CONFIG_BOARD_USE_PROBES),y) ARCHINCLUDES += -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/common diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index a489381b4983..360f7b748ab7 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit a489381b49835ecba6f3b873b5071d882a18152f +Subproject commit 360f7b748ab74f70e10aa8cd4a897b0266c8695f diff --git a/platforms/nuttx/NuttX/extern/pf_crypto b/platforms/nuttx/NuttX/extern/pf_crypto new file mode 160000 index 000000000000..5e18ab60f6f2 --- /dev/null +++ b/platforms/nuttx/NuttX/extern/pf_crypto @@ -0,0 +1 @@ +Subproject commit 5e18ab60f6f23b0ef27bdbf9ff03583ba6ce6791 diff --git a/platforms/nuttx/NuttX/include/queue.h b/platforms/nuttx/NuttX/include/queue.h new file mode 100644 index 000000000000..1b8819873f45 --- /dev/null +++ b/platforms/nuttx/NuttX/include/queue.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +/* Wrapper for nuttx/queue.h */ + +#include diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index b527c6a0af07..add02c254f42 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit b527c6a0af07dc184f4d9bfe5acd9a6546bd04a6 +Subproject commit add02c254f423ab226f5510351822ba500475936 diff --git a/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake b/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake index 94d37b084bea..a4c08874d5d4 100644 --- a/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake +++ b/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake @@ -1,9 +1,9 @@ if(CONFIG_ARCH_DPFPU) message(STATUS "Enabling double FP precision hardware instructions") - set(cpu_flags "-march=rv64gc -mabi=lp64d -mcmodel=medany") + set(cpu_flags "-march=rv64gc -mabi=lp64d -mcmodel=medany -Wl,--no-relax") else() - set(cpu_flags "-march=rv64imac -mabi=lp64 -mcmodel=medany") + set(cpu_flags "-march=rv64imac -mabi=lp64 -mcmodel=medany -Wl,--no-relax") endif() set(CMAKE_C_FLAGS "${cpu_flags}" CACHE STRING "" FORCE) diff --git a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake index 85e5caa00747..3a69862f97c5 100644 --- a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake +++ b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake @@ -2,6 +2,7 @@ set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_VERSION 1) +set(PLATFORM_NAME "nuttx") set(triple arm-none-eabi) set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) diff --git a/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake b/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake index 42f10bff332e..3d957a931757 100644 --- a/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake +++ b/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake @@ -2,6 +2,7 @@ set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_VERSION 1) +set(PLATFORM_NAME "nuttx") set(triple riscv64-unknown-elf) set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 4c5cbab4ef6c..b2b067cfc9e0 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -62,6 +62,7 @@ function(px4_os_add_flags) ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/${CONFIG_ARCH}/src/common ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/apps/include + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/include ) @@ -148,6 +149,9 @@ function(px4_os_determine_build_chip) elseif(CONFIG_ARCH_CHIP_RP2040) set(CHIP_MANUFACTURER "rpi") set(CHIP "rp2040") + elseif(CONFIG_ARCH_CHIP_MPFS) + set(CHIP_MANUFACTURER "microchip") + set(CHIP "mpfs") else() message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.") endif() diff --git a/platforms/nuttx/cmake/upload.cmake b/platforms/nuttx/cmake/upload.cmake index ce776b8a6247..d2ab7ba0d5da 100644 --- a/platforms/nuttx/cmake/upload.cmake +++ b/platforms/nuttx/cmake/upload.cmake @@ -34,8 +34,14 @@ # NuttX CDCACM vendor and product strings set(vendorstr_underscore) set(productstr_underscore) -string(REPLACE " " "_" vendorstr_underscore ${CONFIG_CDCACM_VENDORSTR}) -string(REPLACE " " "_" productstr_underscore ${CONFIG_CDCACM_PRODUCTSTR}) + +if (CONFIG_COMPOSITE_VENDORSTR) + string(REPLACE " " "_" vendorstr_underscore ${CONFIG_COMPOSITE_VENDORSTR}) + string(REPLACE " " "_" productstr_underscore ${CONFIG_COMPOSITE_PRODUCTSTR}) +else() + string(REPLACE " " "_" vendorstr_underscore ${CONFIG_CDCACM_VENDORSTR}) + string(REPLACE " " "_" productstr_underscore ${CONFIG_CDCACM_PRODUCTSTR}) +endif() set(serial_ports) if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux") diff --git a/platforms/nuttx/src/bootloader/common/bl.c b/platforms/nuttx/src/bootloader/common/bl.c index 1e934ed5283a..6b9bb43b8e38 100644 --- a/platforms/nuttx/src/bootloader/common/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -309,7 +309,6 @@ jump_to_app() #ifdef BOOTLOADER_USE_SECURITY crypto_init(); #endif - const image_toc_entry_t *toc_entries; uint8_t len; uint8_t i = 0; @@ -392,6 +391,10 @@ jump_to_app() #endif +#ifdef BOOTLOADER_USE_SECURITY + crypto_deinit(); +#endif + /* just for paranoia's sake */ arch_flash_lock(); diff --git a/platforms/nuttx/src/bootloader/common/crypto.c b/platforms/nuttx/src/bootloader/common/crypto.c index 12bf011dd985..8dd1e3f20d65 100644 --- a/platforms/nuttx/src/bootloader/common/crypto.c +++ b/platforms/nuttx/src/bootloader/common/crypto.c @@ -32,8 +32,10 @@ ****************************************************************************/ #include + #include "image_toc.h" #include "hw_config.h" +#include "crypto.h" #ifdef BOOTLOADER_USE_SECURITY diff --git a/platforms/nuttx/src/bootloader/common/image_toc.c b/platforms/nuttx/src/bootloader/common/image_toc.c index 3d8494f6daf6..01c9cf515f9c 100644 --- a/platforms/nuttx/src/bootloader/common/image_toc.c +++ b/platforms/nuttx/src/bootloader/common/image_toc.c @@ -31,13 +31,12 @@ * ****************************************************************************/ -#include "hw_config.h" - #include #include #include #include +#include "hw_config.h" #include "image_toc.h" #include "bl.h" @@ -45,14 +44,29 @@ /* Helper macros to define flash start and end addresses, based on info from * hw_config.h */ +#ifndef FLASH_START_ADDRESS #define FLASH_START_ADDRESS (APP_LOAD_ADDRESS & (~(BOARD_FLASH_SIZE - 1))) +#endif #define FLASH_END_ADDRESS (FLASH_START_ADDRESS + BOARD_FLASH_SIZE) bool find_toc(const image_toc_entry_t **toc_entries, uint8_t *len) { - const uintptr_t toc_start_u32 = APP_LOAD_ADDRESS + BOOT_DELAY_ADDRESS + 8; - const image_toc_start_t *toc_start = (const image_toc_start_t *)toc_start_u32; - const image_toc_entry_t *entry = (const image_toc_entry_t *)(toc_start_u32 + sizeof(image_toc_start_t)); + uintptr_t toc_start_u32 = TOC_ADDRESS; + const image_toc_start_t *toc_start; + const image_toc_entry_t *entry; + +#ifdef TOC_ADDRESS_ALT + const uint32_t toc_start_u32_alt = TOC_ADDRESS_ALT; + const image_toc_start_t *toc_start_alt = (const image_toc_start_t *)toc_start_u32_alt; + + if (toc_start_alt->magic == TOC_START_MAGIC) { + toc_start_u32 = TOC_ADDRESS_ALT; + } + +#endif + + toc_start = (const image_toc_start_t *)toc_start_u32; + entry = (const image_toc_entry_t *)(toc_start_u32 + sizeof(image_toc_start_t)); int i = 0; uint8_t sig_idx; @@ -72,22 +86,22 @@ bool find_toc(const image_toc_entry_t **toc_entries, uint8_t *len) toc_end_u32 = (uintptr_t)&entry[i] + sizeof(toc_end_magic); /* The number of ToC entries found must be within bounds, and the - * ToC has to lie within the flashable area. Also ensure that - * end > start. + * first entry, containing the ToC, has to lie within the flashable area. + * Also ensure that end > start. */ - if (i <= MAX_TOC_ENTRIES && i > 0 && toc_start_u32 >= (uintptr_t)entry[0].start && toc_end_u32 < (uintptr_t)entry[0].end && - (uintptr_t)entry[0].start == APP_LOAD_ADDRESS && + (uintptr_t)entry[0].start >= FLASH_START_ADDRESS && (uintptr_t)entry[0].end <= (FLASH_END_ADDRESS - sizeof(uintptr_t)) && (uintptr_t)entry[0].end > (uintptr_t)entry[0].start) { + sig_idx = entry[0].signature_idx; /* The signature idx for the first app must be within the TOC, and - * the signature must be within the flash area, not overlapping the - * app. Also ensure that end > start. - */ + * the signature must be within the flash area, not overlapping the + * first entry containing the TOC. Also ensure that end > start. + */ if (sig_idx > 0 && sig_idx < MAX_TOC_ENTRIES && diff --git a/platforms/nuttx/src/bootloader/microchip/CMakeLists.txt b/platforms/nuttx/src/bootloader/microchip/CMakeLists.txt new file mode 100644 index 000000000000..36843c5694ef --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +add_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/bootloader/microchip/mpfs/CMakeLists.txt b/platforms/nuttx/src/bootloader/microchip/mpfs/CMakeLists.txt new file mode 100644 index 000000000000..97f45e3fb332 --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/mpfs/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_bootloader + main.c + systick.c +) + +execute_process( + COMMAND git describe --dirty --all --long + WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} + OUTPUT_VARIABLE VERSION + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + +add_definitions(-DVERSION=\"${VERSION}\") + +target_link_libraries(arch_bootloader + PRIVATE + bootloader_lib + nuttx_arch +) + +target_include_directories(arch_bootloader PRIVATE ${PX4_BOARD_DIR}) +target_link_libraries(nuttx_fs INTERFACE nuttx_c) +target_link_libraries(nuttx_drivers INTERFACE nuttx_fs) diff --git a/platforms/nuttx/src/bootloader/microchip/mpfs/main.c b/platforms/nuttx/src/bootloader/microchip/mpfs/main.c new file mode 100644 index 000000000000..4bf2deab08f4 --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/mpfs/main.c @@ -0,0 +1,1003 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +/* + * MPFS board support for the bootloader. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hw_config.h" +#include "board_type.h" + +#include "bl.h" +#include "uart.h" +#include "lib/flash_cache.h" +#include "riscv_internal.h" + +#include +#include +#include + +#include + +#include "image_toc.h" +#include "crypto.h" + +extern int sercon_main(int c, char **argv); + +#define MK_GPIO_INPUT(def) (((def) & (~(GPIO_OUTPUT))) | GPIO_INPUT) + +#define APP_SIZE_MAX BOARD_FLASH_SIZE + +// Reads/writes to flash are done in this size chunks +#define FLASH_RW_BLOCK 4096 + +/* context passed to cinit */ +#if INTERFACE_USART +# define BOARD_INTERFACE_CONFIG_USART INTERFACE_USART_CONFIG +#endif +#if INTERFACE_USB +# define BOARD_INTERFACE_CONFIG_USB INTERFACE_USB_CONFIG +#endif + +/* Reset reasons */ + +/* Reset was caused by the SCB periphery reset signal*/ +#define RESET_SR_SCB_PERIPH_RESET_MASK (0x01 << 0x0) + +/* Reset was caused by the SCB MSS reset register*/ +#define RESET_SR_SCB_MSS_RESET_MASK (0x01 << 0x1) + +/* Reset was caused by the SCB CPU reset register*/ +#define RESET_SR_SCB_CPU_RESET_MASK (0x01 << 0x2) + +/* Reset was caused by the Risc-V Debugger*/ +#define RESET_SR_DEBUGGER_RESET_MASK (0x01 << 0x3) + +/* Reset was caused by the fabric*/ +#define RESET_SR_FABRIC_RESET_MASK (0x01 << 0x4) + +/* Reset was caused by the watchdog*/ +#define RESET_SR_WDOG_RESET_MASK (0x01 << 0x5) + +/* Indicates that fabric asserted the GPIO reset inputs*/ +#define RESET_SR_GPIO_RESET_MASK (0x01 << 0x6) + +/* Indicates that SCB bus reset occurred (which causes warm reset of MS + S)*/ +#define RESET_SR_SCB_BUS_RESET_MASK (0x01 << 0x7) + +/* Indicates that CPU soft reset occured */ +#define RESET_SR_CPU_SOFT_RESET_MASK (0x01 << 0x8) + +#ifdef CONFIG_MMCSD +static int px4_fd = -1; +static bool sdcard_mounted; +#endif + + +#if BOOTLOADER_VERIFY_UBOOT + +#define UBOOT_BINARY "u-boot_signed.bin" +#define UBOOT_SIGNATURE_SIZE 64 +#else + +#define UBOOT_BINARY "u-boot.bin" +#endif + +static struct mtd_dev_s *mtd = 0; +static struct mtd_geometry_s geo; + +#if defined(CONFIG_MTD_M25P) +static struct spi_dev_s *spinor = 0; +#endif + +static int loader_task = -1; +typedef enum { + UNINITIALIZED = -1, + IN_PROGRESS, + DONE, + SW_UPDATED, + INTERRUPTED, + LOAD_FAIL +} image_loading_status_t; + +static image_loading_status_t loading_status; +static bool u_boot_loaded = false; + +/* board definition */ +struct boardinfo board_info = { + .board_type = BOARD_TYPE, + .board_rev = 0, + .fw_size = 0, + .systick_mhz = 600, +}; + +static void board_init(void); + +/* LED_ACTIVITY == 1, LED_BOOTLOADER == 2 */ +static bool g_led_state[3]; + +/* State of an inserted USB cable */ +static bool usb_connected = false; + +/* PX4 image TOC 'reserved' field for vendor specific info_bits + * Bit 0 marks for whether SBI should be used or not +*/ + +static uint32_t info_bits; + +static uint32_t board_get_reset_reason(void) +{ + return getreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_RESET_SR_OFFSET); +} + +static void board_set_reset_reason(uint32_t reason) +{ + putreg32(reason, MPFS_SYSREG_BASE + MPFS_SYSREG_RESET_SR_OFFSET); +} + +static bool board_test_force_pin(void) +{ +#ifdef BOARD_FORCE_BL_PIN + + if (px4_arch_gpioread(BOARD_FORCE_BL_PIN)) { + return true; + } + +#endif + return false; +} + +uint32_t +board_get_devices(void) +{ + uint32_t devices = BOOT_DEVICES_SELECTION; + + if (usb_connected) { + devices &= BOOT_DEVICES_FILTER_ONUSB; + } + + return devices; +} + +#if defined(CONFIG_MPFS_SPI0) +#define SPI_NOR_CS_FUNC mpfs_spi0_select +#elif defined(CONFIG_MPFS_SPI1) +#define SPI_NOR_CS_FUNC mpfs_spi1_select +#endif + +#if defined(BOARD_PIN_CS_SPINOR) +void +SPI_NOR_CS_FUNC(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + px4_arch_gpiowrite(BOARD_PIN_CS_SPINOR, !selected); +} +#endif + +#ifdef CONFIG_MMCSD +void partition_handler(FAR struct partition_s *part, FAR void *arg) +{ + if (part->index == 0) { + register_blockpartition("/dev/mmcsd0p0", 0, "/dev/mmcsd0", part->firstblock, part->nblocks); + } +} + +static ssize_t load_sdcard_images(const char *name, uint64_t loadaddr) +{ + struct stat file_stat; + + int mmcsd_fd = open(name, O_RDONLY); + + if (mmcsd_fd > 0) { + fstat(mmcsd_fd, &file_stat); + size_t got = read(mmcsd_fd, (void *)loadaddr, file_stat.st_size); + + if (got > 0 && got == (size_t)file_stat.st_size) { + _alert("Loading %s OK\n", name); + close(mmcsd_fd); + return got; + } + } + + _alert("Loading %s failed\n", name); + close(mmcsd_fd); + return -1; +} +#endif + +static void +board_init(void) +{ + /* fix up the max firmware size, we have to read memory to get this */ + board_info.fw_size = APP_SIZE_MAX; + +#if defined(BOARD_FORCE_BL_PIN) + /* configure the force BL pins */ + px4_arch_configgpio(BOARD_FORCE_BL_PIN); +#endif + +#if defined(BOARD_PIN_LED_ACTIVITY) + /* Initialize LEDs */ + px4_arch_configgpio(BOARD_PIN_LED_ACTIVITY); +#endif +#if defined(BOARD_PIN_LED_BOOTLOADER) + /* Initialize LEDs */ + px4_arch_configgpio(BOARD_PIN_LED_BOOTLOADER); +#endif + +#if defined(CONFIG_MTD_M25P) + px4_arch_configgpio(BOARD_PIN_CS_SPINOR); + spinor = mpfs_spibus_initialize(1); + mtd = m25p_initialize(spinor); + + if (mtd) { + if (mtd->ioctl(mtd, MTDIOC_GEOMETRY, + (unsigned long)((uintptr_t)&geo))) { + } + } else { + _alert("ERROR: MTD initialization failure!\n"); + } + +#endif + +#if defined(CONFIG_USBDEV) + mpfs_usbinitialize(); +# if defined(CONFIG_SYSTEM_CDCACM) + sercon_main(0, NULL); +# endif +#endif + +#if defined(CONFIG_MMCSD) + + if (mpfs_board_emmcsd_init() == OK) { + /* Parse partitions */ + parse_block_partition("/dev/mmcsd0", partition_handler, "/sdcard/"); + + /* Mount the sdcard/eMMC */ + sdcard_mounted = mount("/dev/mmcsd0p0", "/sdcard/", "vfat", 0, NULL) == 0 ? true : false; + + if (!sdcard_mounted) { + _alert("SD/eMMC mount failed\n"); + + } else { + _alert("SD/eMMC mounted\n"); + } + +# if defined(CONFIG_USBMSC_COMPOSITE) + + if (board_composite_initialize(0) == OK) { + if (board_composite_connect(0, 0) == NULL) { + _alert("Failed to connect composite\n"); + } + + } else { + _alert("Failed to initialize composite\n"); + } + +# endif + + } else { + _alert("ERROR: Failed to initialize SD card"); + } + +#endif + +} + +void +board_deinit(void) +{ + +#if INTERFACE_USART + up_disable_irq(MPFS_IRQ_MMUART0); + up_disable_irq(MPFS_IRQ_MMUART1); +#endif + +#ifdef CONFIG_MMCSD + + /* Umount the sdcard now if mounted */ + if (sdcard_mounted) { + umount("/sdcard"); + } + + /* If this is left open from flashing, close it now */ + close(px4_fd); + + /* deinitialise the MMC/SD interrupt */ + up_disable_irq(MPFS_IRQ_MMC_MAIN); +#endif + +#ifdef CONFIG_MTD_M25P + mpfs_spibus_uninitialize(spinor); +#endif + +#if defined(BOARD_FORCE_BL_PIN) + /* deinitialise the force BL pin */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_FORCE_BL_PIN)); +#endif + +#if defined(BOARD_PIN_LED_ACTIVITY) + /* Initialize LEDs */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_LED_ACTIVITY)); +#endif +#if defined(BOARD_PIN_LED_BOOTLOADER) + /* Initialize LEDs */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_LED_BOOTLOADER)); +#endif + +#if defined(BOARD_PIN_CS_SPINOR) + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_CS_SPINOR)); +#endif + +#if defined(CONFIG_USBDEV) + up_disable_irq(MPFS_IRQ_USB_MC); +#endif + +} + +static inline void +clock_init(void) +{ + // Done by Nuttx +} + +void +clock_deinit(void) +{ + up_disable_irq(RISCV_IRQ_MTIMER); +} + +void arch_flash_lock(void) +{ +} + +void arch_flash_unlock(void) +{ +} + +inline void arch_setvtor(const uint32_t *address) +{ +} + +inline static uint32_t +flash_func_block_size(void) +{ + return mtd ? geo.blocksize : 4096; +} + +uint32_t +flash_func_sector_size(unsigned sector) +{ + size_t erasesize = mtd ? geo.erasesize : 4096; + + if (sector * erasesize < BOARD_FLASH_SIZE) { + return erasesize; + + } else { + return 0; + } +} + +#ifdef CONFIG_MMCSD +static void create_px4_file(void) +{ + if (sdcard_mounted) { + px4_fd = open("/sdcard/boot/" IMAGE_FN, O_RDWR | O_CREAT | O_TRUNC); + + /* Couldn't open the file, make sure that the directory exists and try to re-open */ + + if (px4_fd < 0) { + int ret = mkdir("/sdcard/boot", S_IRWXU | S_IRWXG | S_IRWXO); + + if (ret < 0) { + _alert("boot directory creation failed %d\n", ret); + } + + px4_fd = open("/sdcard/boot/" IMAGE_FN, O_RDWR | O_CREAT | O_TRUNC, 0644); + } + + if (px4_fd < 0) { + _alert("FATAL: Not able to create px4 fw image!\n"); + + } + } +} +#endif + +void +flash_func_erase_sector(unsigned sector) +{ + + unsigned ss = flash_func_sector_size(sector); + uint64_t *addr = (uint64_t *)((uint64_t)APP_LOAD_ADDRESS + sector * ss); + + /** + * Break any loading process + * + * Wait for loading to stop. Loading always ends up with either + * LOAD_FAIL or DONE, and any other values are intermediate + */ + + irqstate_t flags = px4_enter_critical_section(); + + if (loading_status != LOAD_FAIL && loading_status != DONE) { + loading_status = INTERRUPTED; + } + + px4_leave_critical_section(flags); + + while (loading_status != LOAD_FAIL && loading_status != DONE) { + usleep(1000); + } + +#ifdef CONFIG_MTD_M25P + /* Flash into NOR flash */ + + int ret = MTD_ERASE(mtd, sector, 1); + + if (ret < 0) { + ferr("ERROR: Erase block=%u failed: %d\n", + sector, ret); + } + +#endif + +#if defined(CONFIG_MMCSD) + /* Flashing into eMMC/SD */ + + /* Create the file when erasing the first sector */ + if (sector == 0) { + create_px4_file(); + } + +#endif + + /* Erase the RAM contents */ + + memset(addr, 0xFFFFFFFF, ss); + +} + +#if defined(CONFIG_MTD_M25P) || defined(CONFIG_MMCSD) +static ssize_t flash_write_pages(off_t start, unsigned n_pages, uint8_t *src) +{ + ssize_t ret = 0; +#ifdef CONFIG_MTD_M25P + + ret = MTD_BWRITE(mtd, start, n_pages, src); + + if (ret != n_pages) { + _alert("SPI NOR write error in pages %d-%d\n", start, start + n_pages); + ret = -errno; + } + +#elif defined(CONFIG_MMCSD) + + if (!sdcard_mounted) { + return -EBADF; + } + + /* Write to file, from the app_load_address */ + ret = (ssize_t)lseek(px4_fd, start * flash_func_block_size(), SEEK_SET); + + if (ret >= 0) { + ssize_t bytes = n_pages * flash_func_block_size(); + + ret = write(px4_fd, (void *)src, bytes); + + if (ret != bytes) { + ret = -errno; + _alert("eMMC write error at 0x%x-0x%x\n", start * flash_func_block_size(), + (start + n_pages) * flash_func_block_size()); + } + + } else { + _alert("File lseek fail\n"); + ret = -errno; + } + +#endif + return ret; +} +#endif + +void +flash_func_write_word(uintptr_t address, uint32_t word) +{ + /* Also copy it directly to load address for booting */ + uint32_t *app_load_addr = (uint32_t *)(address + APP_LOAD_ADDRESS); + + *app_load_addr = word; + +#if defined(CONFIG_MTD_M25P) || defined(CONFIG_MMCSD) + + static uintptr_t end_address = 0; + static uintptr_t first_unwritten = 0; + + // start of this block in memory + uint8_t *block_start; + + // total bytes to be written + unsigned bytes; + + int ret = 0; + + if (address > 0 && + ((address + sizeof(uint32_t)) % FLASH_RW_BLOCK) == 0) { + // Every time a full FLASH_RW_BLOCK is received, store it to disk + + block_start = ((uint8_t *)app_load_addr + sizeof(uint32_t)) - FLASH_RW_BLOCK; + bytes = FLASH_RW_BLOCK; + + // first page to be written + off_t write_page = address / flash_func_block_size(); + // total pages to be written + unsigned n_pages = (FLASH_RW_BLOCK / flash_func_block_size()); + // write pages + ret = flash_write_pages(write_page, n_pages, (uint8_t *)block_start); + // store the first unwritten address for the end of image handling + first_unwritten = address + sizeof(uint32_t); + } + + // the address 0 is written last, in the end of flashing + if (address == 0 && word != 0xffffffffu) { + // Write the last incomplete block + bytes = end_address - first_unwritten; + unsigned n_pages = bytes / flash_func_block_size(); + + if (bytes % flash_func_block_size()) { + n_pages += 1; + } + + if (n_pages) { + block_start = (uint8_t *)(APP_LOAD_ADDRESS + first_unwritten); + ret = flash_write_pages(first_unwritten / flash_func_block_size(), n_pages, + block_start); + } + + // re-write the first page + if (ret >= 0) { + block_start = (uint8_t *)APP_LOAD_ADDRESS; + bytes = flash_func_block_size(); + ret = flash_write_pages(0, 1, block_start); + } + } + + // if the writing failed, erase the written data in DRAM, and also + // the first word, in case this was the last write + if (ret < 0) { + memset((uint8_t *)block_start, 0xff, bytes); + *(uint32_t *)APP_LOAD_ADDRESS = 0xffffffffu; + } + + end_address = address + sizeof(uint32_t); +#endif + + /* After the last word has been written, update the loading_status */ + if (address == 0 && word != 0xffffffffu) { +#ifdef CONFIG_MMCSD + close(px4_fd); + px4_fd = -1; +#endif + loading_status = SW_UPDATED; + } +} + +uint32_t flash_func_read_word(uintptr_t address) +{ + uint32_t word; + + address += APP_LOAD_ADDRESS; + word = *(uint32_t *)((uintptr_t)address); + + return word; +} + + +uint32_t +flash_func_read_otp(uintptr_t address) +{ + return 0; +} + +uint32_t get_mcu_id(void) +{ + return 0; +} + +int get_mcu_desc(int max, uint8_t *revstr) +{ + revstr[0] = 1; + revstr[1] = ','; + revstr[2] = 0; + return 3; +} + + +int check_silicon(void) +{ + return 0; +} + +uint32_t +flash_func_read_sn(uintptr_t address) +{ + return 0 ; +} + +void +led_on(unsigned led) +{ + g_led_state[led] = true; + + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, BOARD_LED_ON); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, BOARD_LED_ON); +#endif + break; + } +} + +void +led_off(unsigned led) +{ + g_led_state[led] = false; + + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, BOARD_LED_OFF); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, BOARD_LED_OFF); +#endif + break; + } +} + +void +led_toggle(unsigned led) +{ + g_led_state[led] ^= 1; + + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, g_led_state[led]); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, g_led_state[led]); +#endif + break; + } +} + +/* Make the actual jump to app */ +void +arch_do_jump(const uint32_t *app_base) +{ + + /* PX4 on hart 2 */ +#if CONFIG_MPFS_HART2_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + bool use_sbi = info_bits & INFO_BIT_USE_SBI ? true : false; + _alert("Jump to PX4 0x%lx %s\n", (uint64_t)app_base, use_sbi ? "via SBI" : ""); + mpfs_set_use_sbi(2, use_sbi); + mpfs_set_entrypt(2, (uintptr_t)app_base); + *(volatile uint32_t *)MPFS_CLINT_MSIP2 = 0x01U; +#endif + + /* Linux on harts 1, 3 and 4 */ + if (u_boot_loaded) { +#if CONFIG_MPFS_HART3_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + _alert("Jump to Hart 3 U-boot 0x%lx\n", CONFIG_MPFS_HART3_ENTRYPOINT); + *(volatile uint32_t *)MPFS_CLINT_MSIP3 = 0x01U; +#endif + +// If SMP is used on Linux, the primary core (Hart 3) will boot the secondary +// cores (Hart 1 and 4), so they should not be booted here +#if BOOTLOADER_BOOT_HART_1 +#if CONFIG_MPFS_HART1_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + _alert("Jump to Hart 1 U-boot 0x%lx\n", CONFIG_MPFS_HART1_ENTRYPOINT); + *(volatile uint32_t *)MPFS_CLINT_MSIP1 = 0x01U; +#endif +#endif + +#if BOOTLOADER_BOOT_HART_4 +#if CONFIG_MPFS_HART4_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + _alert("Jump to Hart 4 U-boot 0x%lx\n", CONFIG_MPFS_HART4_ENTRYPOINT); + *(volatile uint32_t *)MPFS_CLINT_MSIP4 = 0x01U; +#endif +#endif + + } + + // TODO. monitor? + while (1) { + usleep(1000000); + } +} + +#if defined(CONFIG_MMCSD) || defined(CONFIG_MTD_M25P) +static size_t get_image_size(void) +{ + const image_toc_entry_t *toc_entries; + const void *end = (const void *)APP_LOAD_ADDRESS; + uint8_t len; + + if (find_toc(&toc_entries, &len)) { + _alert("Found TOC\n"); + + // find the boot image vendor flags and the largest end address + const uint32_t sig = ('B' << 0) | ('O' << 8) | ('O' << 16) | ('T' << 24); + + for (int i = 0; i < len; i++) { + if (*(uint32_t *)(void *)toc_entries[i].name == sig) { + info_bits = toc_entries[i].reserved; + } + + if (toc_entries[i].end > end) { + end = toc_entries[i].end; + } + } + } + + // image size is end address - app start + return (uintptr_t)end - APP_LOAD_ADDRESS; +} +#endif + +#if BOOTLOADER_VERIFY_UBOOT +bool verify_image(void *image_start, size_t image_size, size_t signature_size) +{ + uint8_t signature_idx = 1; + uint16_t index = 0; + + uint8_t *image_end = (uint8_t *)image_start + image_size - signature_size; + uint8_t *signature_start = (uint8_t *)image_start + image_size - signature_size; + uint8_t *signature_end = (uint8_t *)image_start + image_size; + + image_toc_entry_t toc_entries[2] = { + {"IMG ", image_start, image_end, 0, signature_idx, 0, 0, 0}, + {"SIG ", signature_start, signature_end, 0, 0, 0, 0, 0} + }; + + return verify_app(index, toc_entries); +} +#endif + +static int loader_main(int argc, char *argv[]) +{ + ssize_t image_sz = 0; + loading_status = IN_PROGRESS; + +#if defined(CONFIG_MMCSD) + ssize_t ret = 0; + + if (sdcard_mounted) { + ret = load_sdcard_images("/sdcard/boot/" IMAGE_FN, APP_LOAD_ADDRESS); + + if (ret > 0) { + image_sz = get_image_size(); + + if (image_sz > 0) { + _alert("PX4 load success\n"); + } + } + } + +#endif + +#if defined(CONFIG_MTD_M25P) + /* If loading from sdcard didn't succeed, use SPI-NOR */ + + // Read first FLASH_RW_BLOCK size data, search if TOC exists + + if (image_sz == 0) { + const unsigned pgs_per_block = FLASH_RW_BLOCK / flash_func_block_size(); + size_t pages = MTD_BREAD(mtd, 0, pgs_per_block, (uint8_t *)APP_LOAD_ADDRESS); + + if (pages == pgs_per_block) { + image_sz = get_image_size(); + + if (image_sz > 0) { + _alert("Loading from NOR flash\n"); + unsigned reads_left = image_sz / FLASH_RW_BLOCK - 1; + + if (image_sz % FLASH_RW_BLOCK) { + reads_left += 1; + } + + // read the rest in FLASH_RW_BLOCK blocks + for (unsigned i = 1; i < reads_left + 1; i++) { + if (loading_status == INTERRUPTED) { + image_sz = -1; + break; + } + + MTD_BREAD(mtd, i * pgs_per_block, pgs_per_block, ((uint8_t *)APP_LOAD_ADDRESS) + (i * FLASH_RW_BLOCK)); + } + } + } + } + +#endif + +#if defined(CONFIG_OPENSBI) && defined(CONFIG_MMCSD) + ssize_t uboot_size = 0; + + if (sdcard_mounted) { + uboot_size = load_sdcard_images("/sdcard/boot/"UBOOT_BINARY, CONFIG_MPFS_HART3_ENTRYPOINT); + + if (uboot_size > 0) { + u_boot_loaded = true; +#if BOOTLOADER_VERIFY_UBOOT + + if (!verify_image((void *)CONFIG_MPFS_HART3_ENTRYPOINT, uboot_size, UBOOT_SIGNATURE_SIZE)) { + u_boot_loaded = false; + /* Wipe the memory */ + memset((void *)CONFIG_MPFS_HART3_ENTRYPOINT, 0, uboot_size); + _alert("u-boot Authentication Failed\n"); + } + +#endif + + } else { + _alert("u-boot loading failed\n"); + u_boot_loaded = false; + } + } + +#endif + + /* image_sz < 0 means that the load was interrupted due to flashing in progress */ + if (image_sz == 0) { + _alert("No boot image found\n"); + loading_status = LOAD_FAIL; + + } else if (loading_status == IN_PROGRESS) { + _alert("Image loaded succesfully, size %d\n", image_sz); + loading_status = DONE; + + } else { + _alert("Image loading interrupted\n"); + loading_status = LOAD_FAIL; + } + + return 0; +} + +static void start_image_loading(void) +{ + /* Mark that we are waiting for the task to start */ + loading_status = UNINITIALIZED; + + /* create the task */ + loader_task = task_create("loader", SCHED_PRIORITY_MAX - 6, 8192, loader_main, (char *const *)0); + + /* wait for the task to start */ + while (loading_status == UNINITIALIZED) { + usleep(1000); + } +} + +int +bootloader_main(int argc, char *argv[]) +{ + unsigned timeout = BOOTLOADER_DELAY; /* if nonzero, drop out of the bootloader after this time */ + bool try_boot; + _alert("Version: %s\n", VERSION); + + /* do board-specific initialisation */ + board_init(); + + /* configure the clock for bootloader activity */ + clock_init(); + + /* check the bootloader force pin status */ + try_boot = !board_test_force_pin(); + + start_image_loading(); + + /* + * Check the boot reason. In case we came here with SW system reset, + * we stay in bootloader. + * SW system reset is issued in PX4 with "reboot -b" + */ + + uint32_t reset_reason = board_get_reset_reason(); + + /* Is not FABRIC reset and not caused by WDOG? FABRIC reset bit is only + * set in POR, since it is not even connected in FPGA. + * If also CPU_SOFT_RESET is set, we know that this is soft reset from PX4 (reboot -b) + */ + if ((reset_reason & RESET_SR_FABRIC_RESET_MASK) == 0 && + (reset_reason & RESET_SR_WDOG_RESET_MASK) == 0 && + (reset_reason & RESET_SR_CPU_SOFT_RESET_MASK) == RESET_SR_CPU_SOFT_RESET_MASK) { + + /* Don't drop out of the bootloader until something has been uploaded */ + timeout = 0; + } + + /* Clear the reset reason */ + board_set_reset_reason(0); + + /* start the interface */ +#if INTERFACE_USART + cinit(BOARD_INTERFACE_CONFIG_USART, USART); +#endif + +#if INTERFACE_USB + cinit(BOARD_INTERFACE_CONFIG_USB, USB); +#endif + + while (1) { + /* run the bootloader, come back after an app is uploaded or we time out */ + bootloader(timeout); + + /* if the sw was just re-flashed, load the image again */ + if (loading_status == SW_UPDATED) { + timeout = BOOTLOADER_DELAY; + start_image_loading(); + } + + /* look to see if we can boot the app */ + + if (try_boot && loading_status == DONE) { + jump_to_app(); + } + } +} diff --git a/platforms/nuttx/src/bootloader/microchip/mpfs/systick.c b/platforms/nuttx/src/bootloader/microchip/mpfs/systick.c new file mode 100644 index 000000000000..6bb775fdb3ee --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/mpfs/systick.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include + +void arch_systic_init(void) +{ +} + +void arch_systic_deinit(void) +{ +} + +uint8_t systick_get_countflag(void) +{ + return 0; +} + +void systick_set_reload(uint32_t value) +{ +} + + +void systick_set_clocksource(uint8_t clocksource) +{ +} + +void systick_counter_enable(void) +{ +} + +void systick_counter_disable(void) +{ +} + +void systick_interrupt_enable(void) +{ +} + +void systick_interrupt_disable(void) +{ +} diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c index 894834f078fa..d5f575364050 100644 --- a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -634,7 +634,7 @@ arch_do_jump(const uint32_t *app_base) } int -bootloader_main(void) +bootloader_main(int argc, char *argv[]) { bool try_boot = true; /* try booting before we drop to the bootloader */ unsigned timeout = BOOTLOADER_DELAY; /* if nonzero, drop out of the bootloader after this time */ diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index fbccd77a0f1f..3875fa80a59d 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -38,12 +38,12 @@ if(NOT PX4_BOARD MATCHES "io-v2") board_crashdump.c board_dma_alloc.c board_fat_dma_alloc.c - cdc_acm_check.cpp console_buffer.cpp cpuload.cpp gpio.c print_load.cpp tasks.cpp + px4_atomic.cpp px4_nuttx_impl.cpp px4_init.cpp px4_manifest.cpp diff --git a/platforms/nuttx/src/px4/common/board_crashdump.c b/platforms/nuttx/src/px4/common/board_crashdump.c index 90e541ae0e24..b8de3591362a 100644 --- a/platforms/nuttx/src/px4/common/board_crashdump.c +++ b/platforms/nuttx/src/px4/common/board_crashdump.c @@ -232,7 +232,7 @@ static void copy_reverse(stack_word_t *dest, stack_word_t *src, int size) static uint32_t *__attribute__((noinline)) __sdata_addr(void) { - return &_sdata; + return (uint32_t *)(uint32_t)_sdata; } diff --git a/platforms/nuttx/src/px4/common/board_ioctl.c b/platforms/nuttx/src/px4/common/board_ioctl.c index 9db26b15a09a..d510b743488a 100644 --- a/platforms/nuttx/src/px4/common/board_ioctl.c +++ b/platforms/nuttx/src/px4/common/board_ioctl.c @@ -42,14 +42,22 @@ #include #include "board_config.h" -#include +#include + #include -FAR const struct builtin_s g_kernel_builtins[] = { +struct kernel_builtin_s { + const char *name; /* Invocation name and as seen under /sbin/ */ + int priority; /* Use: SCHED_PRIORITY_DEFAULT */ + int stacksize; /* Desired stack size */ + main_t main; /* Entry point: main(int argc, char *argv[]) */ +}; + +const struct kernel_builtin_s g_kernel_builtins[] = { #include }; -const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(struct builtin_s); +const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(g_kernel_builtins[0]); static struct { ioctl_ptr_t ioctl_func; @@ -107,10 +115,11 @@ int board_ioctl(unsigned int cmd, uintptr_t arg) static int launch_kernel_builtin(int argc, char **argv) { int i; - FAR const struct builtin_s *builtin = NULL; + const struct kernel_builtin_s *builtin = NULL; + char *name = basename(argv[0]); for (i = 0; i < g_n_kernel_builtins; i++) { - if (!strcmp(g_kernel_builtins[i].name, argv[0])) { + if (!strcmp(g_kernel_builtins[i].name, name)) { builtin = &g_kernel_builtins[i]; break; } diff --git a/platforms/nuttx/src/px4/common/hrt_ioctl.c b/platforms/nuttx/src/px4/common/hrt_ioctl.c index e9b6e82efd81..b6d93730e4cf 100644 --- a/platforms/nuttx/src/px4/common/hrt_ioctl.c +++ b/platforms/nuttx/src/px4/common/hrt_ioctl.c @@ -42,40 +42,132 @@ #include #include +#include +#include #ifndef MODULE_NAME # define MODULE_NAME "hrt_ioctl" #endif -#define HRT_ENTRY_QUEUE_MAX_SIZE 3 -static px4_sem_t g_wait_sem; -static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE]; -static int hrt_entry_queued = 0; -static bool suppress_entry_queue_error = false; -static bool hrt_entry_queue_error = false; +struct usr_hrt_call { + struct sq_entry_s list_item; /* List head for local sl_list */ + struct hrt_call entry; /* Kernel side entry for HRT driver */ + struct hrt_call *usr_entry; /* Reference to user side entry */ +}; -void hrt_usr_call(void *arg) +static sq_queue_t callout_queue; +static sq_queue_t callout_freelist; +static sq_queue_t callout_inflight; + +/* Find (pop) first entry for user from queue, the queue must be locked prior */ + +struct usr_hrt_call *pop_user(sq_queue_t *queue, const px4_hrt_handle_t handle) { - // This is called from hrt interrupt - if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) { - next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg; + sq_entry_t *queued; + + sq_for_every(queue, queued) { + struct usr_hrt_call *e = (void *)queued; + + if (e->entry.callout_sem == handle) { + sq_rem(queued, queue); + return e; + } + } + + return NULL; +} + +/* Find (pop) entry from queue, the queue must be locked prior */ + +struct usr_hrt_call *pop_entry(sq_queue_t *queue, const px4_hrt_handle_t handle, struct hrt_call *entry) +{ + sq_entry_t *queued; + + sq_for_every(queue, queued) { + struct usr_hrt_call *e = (void *)queued; + + if (e->usr_entry == entry && e->entry.callout_sem == handle) { + sq_rem(queued, queue); + return e; + } + } + + return NULL; +} + +/** + * Copy user entry to kernel space. Either re-uses existing one or if none can + * be found, creates a new. + * + * handle : user space handle to identify who is behind the HRT request + * entry : user space HRT entry + * callout : user callback + * arg : user argument passed in callback + */ +static struct usr_hrt_call *dup_entry(const px4_hrt_handle_t handle, struct hrt_call *entry, hrt_callout callout, + void *arg) +{ + struct usr_hrt_call *e = NULL; + + irqstate_t flags = px4_enter_critical_section(); + + /* check if this is already queued */ + e = pop_entry(&callout_queue, handle, entry); + + /* it was not already queued, get from freelist */ + if (!e) { + e = (void *)sq_remfirst(&callout_freelist); + } + + px4_leave_critical_section(flags); + + if (!e) { + /* Allocate a new kernel side item for the user call */ + + e = kmm_malloc(sizeof(struct usr_hrt_call)); + } + + if (e) { + + /* Store the user side callout function and argument to the user's handle */ + entry->callout = callout; + entry->arg = arg; + + /* Store reference to the kernel side entry to the user side struct and + * references to the semaphore and user side entry to the kernel side item + */ + + e->entry.callout_sem = handle; + e->usr_entry = entry; + + /* Add this to the callout_queue list */ + flags = px4_enter_critical_section(); + sq_addfirst(&e->list_item, &callout_queue); + px4_leave_critical_section(flags); } else { - hrt_entry_queue_error = true; + PX4_ERR("out of memory"); + } - px4_sem_post(&g_wait_sem); + return e; +} + +void hrt_usr_call(void *arg) +{ + // This is called from hrt interrupt + struct usr_hrt_call *e = (struct usr_hrt_call *)arg; + sq_addfirst(&e->list_item, &callout_inflight); + px4_sem_post(e->entry.callout_sem); } int hrt_ioctl(unsigned int cmd, unsigned long arg); void hrt_ioctl_init(void) { - /* Create a semaphore for handling hrt driver callbacks */ - px4_sem_init(&g_wait_sem, 0, 0); - - /* this is a signalling semaphore */ - px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE); + sq_init(&callout_queue); + sq_init(&callout_freelist); + sq_init(&callout_inflight); /* register ioctl callbacks */ px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl); @@ -105,26 +197,32 @@ hrt_ioctl(unsigned int cmd, unsigned long arg) switch (cmd) { case HRT_WAITEVENT: { irqstate_t flags; - px4_sem_wait(&g_wait_sem); + struct hrt_boardctl *ioc_parm = (struct hrt_boardctl *)arg; + px4_sem_t *callout_sem = (px4_sem_t *)ioc_parm->handle; + struct usr_hrt_call *e; + px4_sem_wait(callout_sem); + /* Atomically update the pointer to user side hrt entry */ flags = px4_enter_critical_section(); + e = pop_user(&callout_inflight, callout_sem); - /* This should be always true, but check it anyway */ - if (hrt_entry_queued > 0) { - *(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued]; - next_hrt_entry[hrt_entry_queued] = NULL; + if (e) { + ioc_parm->callout = e->usr_entry->callout; + ioc_parm->arg = e->usr_entry->arg; + + // If the period is 0, the callout is no longer queued by hrt driver + // move it back to freelist + if (e->entry.period == 0) { + sq_rem((sq_entry_t *)e, &callout_queue); + sq_addfirst((sq_entry_t *)e, &callout_freelist); + + } } else { - hrt_entry_queue_error = true; + PX4_ERR("HRT_WAITEVENT error no entry"); } px4_leave_critical_section(flags); - - /* Warn once for entry queue being full */ - if (hrt_entry_queue_error && !suppress_entry_queue_error) { - PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued); - suppress_entry_queue_error = true; - } } break; @@ -132,21 +230,46 @@ hrt_ioctl(unsigned int cmd, unsigned long arg) *(hrt_abstime *)arg = hrt_absolute_time(); break; - case HRT_CALL_AFTER: - hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry); + case HRT_CALL_AFTER: { + struct usr_hrt_call *e = dup_entry(h->handle, h->entry, h->callout, h->arg); + + if (e) { + hrt_call_after(&e->entry, h->time, (hrt_callout)hrt_usr_call, e); + } + } break; - case HRT_CALL_AT: - hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry); + case HRT_CALL_AT: { + struct usr_hrt_call *e = dup_entry(h->handle, h->entry, h->callout, h->arg); + + if (e) { + hrt_call_at(&e->entry, h->time, (hrt_callout)hrt_usr_call, e); + } + } break; - case HRT_CALL_EVERY: - hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry); + case HRT_CALL_EVERY: { + struct usr_hrt_call *e = dup_entry(h->handle, h->entry, h->callout, h->arg); + + if (e) { + hrt_call_every(&e->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, e); + } + } break; case HRT_CANCEL: if (h && h->entry) { - hrt_cancel(h->entry); + /* Find the user entry */ + irqstate_t flags = px4_enter_critical_section(); + struct usr_hrt_call *e = pop_entry(&callout_queue, h->handle, h->entry); + + if (e) { + hrt_cancel(&e->entry); + sq_addfirst((sq_entry_t *)e, &callout_freelist); + + } + + px4_leave_critical_section(flags); } else { PX4_ERR("HRT_CANCEL called with NULL entry"); @@ -164,6 +287,58 @@ hrt_ioctl(unsigned int cmd, unsigned long arg) reset_latency_counters(); break; + case HRT_REGISTER: { + px4_sem_t *callback_sem = kmm_malloc(sizeof(px4_sem_t)); + + /* Create a semaphore for handling hrt driver callbacks */ + if (px4_sem_init(callback_sem, 0, 0) == 0) { + + /* this is a signalling semaphore */ + px4_sem_setprotocol(callback_sem, SEM_PRIO_NONE); + *(px4_sem_t **)arg = callback_sem; + + } else { + *(px4_sem_t **)arg = NULL; + return -ENOMEM; + } + + } + + break; + + case HRT_UNREGISTER: { + px4_sem_t *callback_sem = *(px4_sem_t **)arg; + struct usr_hrt_call *e; + irqstate_t flags; + + flags = px4_enter_critical_section(); + + while ((e = (void *)sq_remfirst(&callout_queue))) { + if (callback_sem == e->entry.callout_sem) { + hrt_cancel(&e->entry); + /* Remove potential inflight entry as well */ + sq_rem(&e->list_item, &callout_inflight); + kmm_free(e); + } + } + + px4_sem_destroy(callback_sem); + + px4_leave_critical_section(flags); + + *(px4_sem_t **)arg = NULL; + kmm_free(callback_sem); + } + break; + + case HRT_ABSTIME_BASE: { +#ifdef PX4_USERSPACE_HRT + *(uintptr_t *)arg = hrt_absolute_time_usr_base(); +#else + *(uintptr_t *)arg = NULL; +#endif + } + break; default: return -EINVAL; } diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/atomic_block.h b/platforms/nuttx/src/px4/common/include/px4_platform/atomic_block.h new file mode 100644 index 000000000000..f95d08ed858c --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/px4_platform/atomic_block.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace px4 +{ + +class atomic_block +{ +public: + atomic_block(); + ~atomic_block(); + void start(); + void finish(); +private: + union { + px4_sem_t _lock; + irqstate_t _irqlock; + }; +}; + +} diff --git a/platforms/nuttx/src/px4/common/nuttx_random.c b/platforms/nuttx/src/px4/common/nuttx_random.c index 1339aa916904..e0ef28a6f527 100644 --- a/platforms/nuttx/src/px4/common/nuttx_random.c +++ b/platforms/nuttx/src/px4/common/nuttx_random.c @@ -33,12 +33,14 @@ #include -#if defined(CONFIG_CRYPTO_RANDOM_POOL) size_t px4_get_secure_random(uint8_t *out, size_t outlen) { +#if defined(CONFIG_CRYPTO_RANDOM_POOL) /* TODO: can getrandom fail?? */ arc4random_buf(out, outlen); return outlen; -} +#else + return 0; #endif +} diff --git a/platforms/nuttx/src/px4/common/print_load.cpp b/platforms/nuttx/src/px4/common/print_load.cpp index 8c6486453ade..230b23acd59c 100644 --- a/platforms/nuttx/src/px4/common/print_load.cpp +++ b/platforms/nuttx/src/px4/common/print_load.cpp @@ -202,14 +202,14 @@ void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb if (system_load.tasks[i].tcb->pid == 0) { stack_size = (CONFIG_ARCH_INTERRUPTSTACK & ~3); - stack_free = up_check_intstack_remain(); + stack_free = (CONFIG_ARCH_INTERRUPTSTACK & ~3) - up_check_intstack(); } else { - stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb); + stack_free = system_load.tasks[i].tcb->adj_stack_size - up_check_tcbstack(system_load.tasks[i].tcb); } #else - stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb); + stack_free = system_load.tasks[i].tcb->adj_stack_size - up_check_tcbstack(system_load.tasks[i].tcb); #endif #if CONFIG_ARCH_BOARD_SIM || !defined(CONFIG_PRIORITY_INHERITANCE) diff --git a/platforms/nuttx/src/px4/common/px4_atomic.cpp b/platforms/nuttx/src/px4/common/px4_atomic.cpp new file mode 100644 index 000000000000..819dbc6ecc8b --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_atomic.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include + +namespace px4 +{ + +atomic_block::atomic_block(void) +{ + _irqlock = 0; +} + +atomic_block::~atomic_block(void) +{ + +} + +void atomic_block::start(void) +{ + _irqlock = enter_critical_section(); +} + +void atomic_block::finish(void) +{ + leave_critical_section(_irqlock); +} + +} diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index fb371b9093a9..baa35d0f648f 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -61,14 +61,18 @@ #include #endif +#ifndef I2C_RESET_SPEED +#define I2C_RESET_SPEED I2C_SPEED_STANDARD +#endif + extern void cdcacm_init(void); #if !defined(CONFIG_BUILD_FLAT) typedef CODE void (*initializer_t)(void); -extern initializer_t _sinit; -extern initializer_t _einit; -extern uint32_t _stext; -extern uint32_t _etext; +extern initializer_t _sinit[]; +extern initializer_t _einit[]; +extern uint8_t _stext[]; +extern uint8_t _etext[]; static void cxx_initialize(void) { @@ -76,7 +80,7 @@ static void cxx_initialize(void) /* Visit each entry in the initialization table */ - for (initp = &_sinit; initp != &_einit; initp++) { + for (initp = _sinit; initp != _einit; initp++) { initializer_t initializer = *initp; /* Make sure that the address is non-NULL and lies in the text @@ -84,8 +88,8 @@ static void cxx_initialize(void) * NULL values or counts in the initialization table. */ - if ((FAR void *)initializer >= (FAR void *)&_stext && - (FAR void *)initializer < (FAR void *)&_etext) { + if ((FAR void *)initializer >= (FAR void *)_stext && + (FAR void *)initializer < (FAR void *)_etext) { initializer(); } } @@ -150,7 +154,7 @@ int px4_platform_init() buf[0] = 0x06; // software reset i2c_msg_s msg{}; - msg.frequency = I2C_SPEED_STANDARD; + msg.frequency = I2C_RESET_SPEED; msg.addr = 0x00; // general call address msg.buffer = &buf[0]; msg.length = 1; diff --git a/platforms/nuttx/src/px4/common/px4_kmmap.c b/platforms/nuttx/src/px4/common/px4_kmmap.c new file mode 100644 index 000000000000..68a26e993a30 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_kmmap.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * px4_kmmap.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#ifndef CONFIG_ARCH_PGPOOL_MAPPING +# error "kmmap needs CONFIG_ARCH_PGPOOL_MAPPING" +#endif + +#if CONFIG_ARCH_PGPOOL_PBASE != CONFIG_ARCH_PGPOOL_VBASE +# error "kmmap needs CONFIG_ARCH_PGPOOL_PBASE=CONFIG_ARCH_PGPOOL_VBASE mapping" +#endif + +#define MAP_FAILED ((void*)-1) + +/* This is naughty, but only option as these are in NuttX private headers */ + +extern int inode_lock(void); +extern void inode_unlock(void); + +struct shmfs_object_s { + size_t length; + void *paddr[]; +}; + +void *px4_mmap(void *start, size_t length, int prot, int flags, int fd, off_t offset) +{ + struct file *filep; + struct shmfs_object_s *object; + int ret; + + if (fs_getfilep(fd, &filep) < 0) { + ret = -EBADF; + goto errout; + } + + /* Limitation: only 1 page can be mapped. To map more pages, need more logic + * in place (define shared memory are for kernel, keep mappings in list, etc + */ + + if (length > MM_PGSIZE) { + ret = -ENOMEM; + goto errout; + } + + ret = inode_lock(); + + if (ret < 0) { + goto errout; + } + + /* Return the physical address */ + + object = (struct shmfs_object_s *)filep->f_inode->i_private; + + if (!object) { + ret = -EINVAL; + goto errout_with_lock; + } + + filep->f_inode->i_crefs++; + inode_unlock(); + return object->paddr[0]; + +errout_with_lock: + inode_unlock(); +errout: + set_errno(-ret); + return MAP_FAILED; +} + +int px4_munmap(void *start, size_t length) +{ + /* There is no need for unmap on the kernel side */ + + return OK; +} diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 7f76502e6842..78f360161733 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -11,7 +11,7 @@ add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp px4_userspace_init.cpp px4_usr_crypto.cpp - px4_mtd.cpp + usr_atomic.cpp usr_board_ctrl.c usr_hrt.cpp usr_mcu_version.cpp @@ -19,7 +19,6 @@ add_library(px4_layer target_link_libraries(px4_layer PRIVATE - m nuttx_c nuttx_xx nuttx_mm @@ -67,3 +66,8 @@ endif() add_dependencies(px4_kernel_layer prebuild_targets) target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__) target_link_libraries(px4_kernel_layer PUBLIC px4_board_ctrl) + +if (CONFIG_BUILD_KERNEL) + target_sources(px4_layer PRIVATE usr_mmap.c) + target_sources(px4_kernel_layer PRIVATE px4_kmmap.c) +endif() diff --git a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp index f5d9882ca23a..7db9bbd1c72a 100644 --- a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -54,7 +55,7 @@ extern "C" void px4_userspace_init(void) px4::WorkQueueManagerStart(); - uorb_start(); + px4_log_initialize(); #if defined(CONFIG_SYSTEM_CDCACM) cdcacm_init(); diff --git a/platforms/nuttx/src/px4/common/tasks.cpp b/platforms/nuttx/src/px4/common/tasks.cpp index 9ad860dc182f..85f1fad5d1c9 100644 --- a/platforms/nuttx/src/px4/common/tasks.cpp +++ b/platforms/nuttx/src/px4/common/tasks.cpp @@ -88,16 +88,21 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_ int px4_task_delete(int pid) { - return task_delete(pid); + int ret = OK; + + if (pid == getpid()) { + // Commit suicide + exit(EXIT_SUCCESS); + + } else { + // Politely ask someone else to kill themselves + ret = kill(pid, SIGKILL); + } + + return ret; } const char *px4_get_taskname(void) { -#if CONFIG_TASK_NAME_SIZE > 0 && (defined(__KERNEL__) || defined(CONFIG_BUILD_FLAT)) - FAR struct tcb_s *thisproc = nxsched_self(); - - return thisproc->name; -#else - return "app"; -#endif + return getprogname(); } diff --git a/platforms/nuttx/src/px4/common/usr_atomic.cpp b/platforms/nuttx/src/px4/common/usr_atomic.cpp new file mode 100644 index 000000000000..b1221e4e1132 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_atomic.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include + +namespace px4 +{ + +atomic_block::atomic_block(void) +{ + px4_sem_init(&_lock, 0, 1); +} + +atomic_block::~atomic_block(void) +{ + px4_sem_destroy(&_lock); +} + +void atomic_block::start(void) +{ + do {} while (px4_sem_wait(&_lock) != 0); +} + +void atomic_block::finish(void) +{ + px4_sem_post(&_lock); +} + +} diff --git a/platforms/nuttx/src/px4/common/usr_hrt.cpp b/platforms/nuttx/src/px4/common/usr_hrt.cpp index d57712a32d45..80a95f380645 100644 --- a/platforms/nuttx/src/px4/common/usr_hrt.cpp +++ b/platforms/nuttx/src/px4/common/usr_hrt.cpp @@ -61,6 +61,20 @@ #include static px4_task_t g_usr_hrt_task = -1; +static px4_hrt_handle_t g_hrt_client_handle; + +#ifdef PX4_USERSPACE_HRT +static uintptr_t g_abstime_base; +#endif + +/** + * Wrapper for atexit() + */ +static void hrt_stop(void) +{ + px4_task_delete(g_usr_hrt_task); + boardctl(HRT_UNREGISTER, (uintptr_t)&g_hrt_client_handle); +} /** * Fetch a never-wrapping absolute time value in microseconds from @@ -69,20 +83,14 @@ static px4_task_t g_usr_hrt_task = -1; hrt_abstime hrt_absolute_time(void) { +#ifndef PX4_USERSPACE_HRT hrt_abstime abstime = 0; boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime); return abstime; -} - -/** - * Store the absolute time in an interrupt-safe fashion - */ -void -hrt_store_absolute_time(volatile hrt_abstime *t) -{ - irqstate_t flags = px4_enter_critical_section(); - *t = hrt_absolute_time(); - px4_leave_critical_section(flags); +#else + assert(g_abstime_base); + return getreg64(g_abstime_base); +#endif } /** @@ -91,15 +99,22 @@ hrt_store_absolute_time(volatile hrt_abstime *t) int event_thread(int argc, char *argv[]) { - struct hrt_call *entry = NULL; + struct hrt_boardctl ioc_parm { + .handle = g_hrt_client_handle, + .entry = nullptr, + .time = 0, + .interval = 0, + .callout = nullptr, + .arg = nullptr + }; while (1) { /* Wait for hrt tick */ - boardctl(HRT_WAITEVENT, (uintptr_t)&entry); + boardctl(HRT_WAITEVENT, (uintptr_t)&ioc_parm); /* HRT event received, dispatch */ - if (entry) { - entry->usr_callout(entry->usr_arg); + if (ioc_parm.callout) { + ioc_parm.callout(ioc_parm.arg); } } @@ -111,7 +126,7 @@ event_thread(int argc, char *argv[]) */ bool hrt_request_stop() { - px4_task_delete(g_usr_hrt_task); + hrt_stop(); return true; } @@ -121,8 +136,15 @@ bool hrt_request_stop() void hrt_init(void) { - px4_register_shutdown_hook(hrt_request_stop); - g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL); + boardctl(HRT_REGISTER, (uintptr_t)&g_hrt_client_handle); +#ifdef PX4_USERSPACE_HRT + boardctl(HRT_ABSTIME_BASE, (uintptr_t)&g_abstime_base); +#endif + if (g_hrt_client_handle) { + atexit(hrt_stop); + g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, PX4_STACK_ADJUSTED(1024), + event_thread, NULL); + } } /** @@ -131,13 +153,14 @@ hrt_init(void) void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { - hrt_boardctl_t ioc_parm; - ioc_parm.entry = entry; - ioc_parm.time = delay; - ioc_parm.callout = callout; - ioc_parm.arg = arg; - entry->usr_callout = callout; - entry->usr_arg = arg; + struct hrt_boardctl ioc_parm { + .handle = g_hrt_client_handle, + .entry = entry, + .time = delay, + .interval = 0, + .callout = callout, + .arg = arg + }; boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm); } @@ -148,14 +171,14 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) { - hrt_boardctl_t ioc_parm; - ioc_parm.entry = entry; - ioc_parm.time = calltime; - ioc_parm.interval = 0; - ioc_parm.callout = callout; - ioc_parm.arg = arg; - entry->usr_callout = callout; - entry->usr_arg = arg; + hrt_boardctl_t ioc_parm{ + .handle = g_hrt_client_handle, + .entry = entry, + .time = calltime, + .interval = 0, + .callout = callout, + .arg = arg + }; boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm); } @@ -166,14 +189,14 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_boardctl_t ioc_parm; - ioc_parm.entry = entry; - ioc_parm.time = delay; - ioc_parm.interval = interval; - ioc_parm.callout = callout; - ioc_parm.arg = arg; - entry->usr_callout = callout; - entry->usr_arg = arg; + hrt_boardctl_t ioc_parm { + .handle = g_hrt_client_handle, + .entry = entry, + .time = delay, + .interval = interval, + .callout = callout, + .arg = arg, + }; boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm); } @@ -184,7 +207,12 @@ hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, void hrt_cancel(struct hrt_call *entry) { - boardctl(HRT_CANCEL, (uintptr_t)entry); + hrt_boardctl_t ioc_parm { + .handle = g_hrt_client_handle, + .entry = entry, + }; + + boardctl(HRT_CANCEL, (uintptr_t)&ioc_parm); } void diff --git a/platforms/nuttx/src/px4/common/usr_mmap.c b/platforms/nuttx/src/px4/common/usr_mmap.c new file mode 100644 index 000000000000..496a588fbd48 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_mmap.c @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include +#include + +void *px4_mmap(void *start, size_t length, int prot, int flags, int fd, off_t offset) +{ + return mmap(start, length, prot, flags, fd, offset); +} + +int px4_munmap(void *start, size_t length) +{ + return munmap(start, length); +} diff --git a/platforms/nuttx/src/px4/microchip/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/CMakeLists.txt new file mode 100644 index 000000000000..26e4fc545a97 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + + +add_subdirectory(${PX4_CHIP}) + diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/adc/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/microchip_common/adc/CMakeLists.txt new file mode 100644 index 000000000000..a78db53e8b5a --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/adc/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2022 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/adc/adc.cpp b/platforms/nuttx/src/px4/microchip/microchip_common/adc/adc.cpp new file mode 100644 index 000000000000..dd0984ba0ef6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/adc/adc.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#include +#include +#include + +int px4_arch_adc_init(uint32_t base_address) +{ + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + +} + +uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + return 0xffffffffu; +} + +float px4_arch_adc_reference_v() +{ + return 0.0f; +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + + return 0; + +} + +uint32_t px4_arch_adc_dn_fullcount() +{ + return 0; +} diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..58f666db1d07 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..afa84bf34e6b --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/micro_hal.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Techonology Innovation Institute. 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. + * + ****************************************************************************/ +#pragma once + +#include diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..924824f41801 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/spi_hw_description.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#if defined(CONFIG_SPI) + +static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + px4_spi_bus_device_t ret{}; + + ret.cs_gpio = getGPIOBank(cs_gpio.bank) | getGPIOPin(cs_gpio.pin) | (GPIO_OUTPUT | GPIO_BUFFER_ENABLE); + + if (drdy_gpio.bank != GPIO::BankInvalid) { + ret.drdy_gpio = getGPIOBank(drdy_gpio.bank) | getGPIOPin(drdy_gpio.pin) | GPIO_INPUT; + } + + if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external) + // Construct device id including cs_gpio pin information into address field + ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid) | (cs_gpio.pin << 8); + + } else { // it's a NuttX device (e.g. SPIDEV_FLASH(0)) + ret.devid = devid; + } + + ret.devtype_driver = PX4_SPI_DEV_ID(devid); + + return ret; +} + +static inline constexpr px4_spi_bus_t initSPIBusInternal(SPI::Bus bus, const px4_spi_bus_devices_t &devices, + GPIO::GPIOPin power_enable = {}) +{ + px4_spi_bus_t ret{}; + + ret.requires_locking = false; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + ret.devices[i] = devices.devices[i]; + + // check that the same device is configured only once (the chip-select code depends on that) + for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) { + if (ret.devices[j].cs_gpio != 0) { + constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times"); + } + } + + if (ret.devices[i].cs_gpio != 0) { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) + if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + ret.requires_locking = true; + } + } + } + + ret.bus = (int)bus; + ret.is_external = false; + + if (power_enable.bank != GPIO::BankInvalid) { + ret.power_enable_gpio = getGPIOBank(power_enable.bank) | getGPIOPin(power_enable.pin) | GPIO_OUTPUT; + } + + return ret; +} + +// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments +struct bus_device_external_cfg_array_t { + SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES]; +}; + +static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices) +{ + px4_spi_bus_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (devices.devices[i].cs_gpio.bank == GPIO::BankInvalid) { + break; + } + + ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio); + } + + ret.bus = (int)bus; + ret.is_external = true; + ret.requires_locking = false; // external buses are never accessed by NuttX drivers + return ret; + + return ret; +} + +static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + SPI::bus_device_external_cfg_t ret{}; + ret.cs_gpio = cs_gpio; + ret.drdy_gpio = drdy_gpio; + return ret; +} + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt new file mode 100644 index 000000000000..5ccd6eebe70f --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + + +add_subdirectory(board_critmon) +add_subdirectory(board_reset board_reset) +add_subdirectory(hrt hrt) +add_subdirectory(version version) +add_subdirectory(tone_alarm tone_alarm) +add_subdirectory(../microchip_common/adc adc) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/CMakeLists.txt new file mode 100644 index 000000000000..c66b17e5bce6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_board_critmon + board_critmon.c +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/board_critmon.c new file mode 100644 index 000000000000..0d115e641ce1 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/board_critmon.c @@ -0,0 +1,67 @@ +/************************************************************************************ + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * Author: Jukka Laitinen + * + * 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 + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#error "critmon not supported yet" + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +uint32_t up_critmon_gettime(void) +{ + return 0; +} + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +{ +} + +#endif /* CONFIG_SCHED_CRITMONITOR || CONFIG_SCHED_IRQMONITOR */ diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/CMakeLists.txt new file mode 100644 index 000000000000..ef783ad14199 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_board_reset + board_reset.cpp +) + +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp new file mode 100644 index 000000000000..0ff4cbb073e5 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * Author: @author Jukka Laitinen + * + * 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 board_reset.cpp + * Implementation of Microchip PolarFire based Board RESET API + */ + +#include +#include +#include + +extern "C" void __start(void); + +static void board_reset_enter_bootloader() +{ + /* Reset the whole SoC */ + + up_systemreset(); +} + +int board_reset(int status) +{ + +#if defined(BOARD_HAS_ON_RESET) + board_on_reset(status); +#endif + + if (status == 1) { + board_reset_enter_bootloader(); + } + + /* Just reboot via reset vector */ + + __start(); + + return 0; +} diff --git a/platforms/nuttx/src/px4/microchip/mpfs/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/hrt/CMakeLists.txt new file mode 100644 index 000000000000..2d59cdb8b007 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/hrt/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_hrt + hrt.c +) + + diff --git a/platforms/nuttx/src/px4/microchip/mpfs/hrt/hrt.c b/platforms/nuttx/src/px4/microchip/mpfs/hrt/hrt.c new file mode 100644 index 000000000000..21dd8bc7591f --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/hrt/hrt.c @@ -0,0 +1,471 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + * Author: Jani Paalijärvi + * + * High-resolution timer callouts and timekeeping. + * The implementation is based on 32-bit decrementing counter (MSTIMER) + * which is used in periodic mode and generates an interrupt + * when timer reaches zero. + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "mpfs_memorymap.h" +#include "hardware/mpfs_timer.h" + +#define getreg32(a) (*(volatile uint32_t *)(a)) +#define putreg32(v,a) (*(volatile uint32_t *)(a) = (v)) + +#ifdef CONFIG_DEBUG_HRT +# define hrtinfo _info +#else +# define hrtinfo(x...) +#endif + +/* This assumes MTIMER count at 1MHz in MPFS */ + +#if MPFS_MSS_RTC_TOGGLE_CLK != 1000000 +# error This driver currently assumes that MTIMER runs at 1MHz +#endif + +#define CLOCK_RATE_MHZ (MPFS_MSS_APB_AHB_CLK / 1000000) + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ + +#define HRT_COUNTS_TO_TIME(_c) ((_c) / CLOCK_RATE_MHZ) +#define HRT_TIME_TO_COUNTS(_a) ((_a) * CLOCK_RATE_MHZ) + +#define HRT_INTERVAL_MIN 50UL // 50 microseconds +#define HRT_INTERVAL_MAX HRT_COUNTS_TO_TIME(0xFFFFFFFF) // ~28.6s at 150MHz timer + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* last loaded value for irq latency calculation */ +static hrt_abstime loadval; + +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT 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, void *args); +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); + +/** + * function to set new time to the the next interrupt. + * + * Always call with interrupts disabled. + */ + +inline static void hrt_set_new_deadline(uint32_t deadline) +{ + /* load the new deadline into register and store it locally */ + putreg32(HRT_TIME_TO_COUNTS(deadline), MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1LOADVAL_OFFSET); + loadval = hrt_absolute_time() + deadline; +} + +/** + * Initialize the timer we are going to use. + */ +static void +hrt_tim_init(void) +{ + /* e.g. connect hrt_tim_isr to a timer vector, initialize the timer */ + + /* attach irq */ + int ret; + ret = irq_attach(MPFS_IRQ_TIMER1, hrt_tim_isr, NULL); + + if (ret == OK) { + + /* Assumes that the clock for timer is enabled and not in reset */ + + /* set an initial timeout to 1 ms*/ + hrt_set_new_deadline(1000); + + /* enable interrupt for timer, set periodic mode and enable timer */ + putreg32((MPFS_MSTIMER_INTEN_MASK | MPFS_MSTIMER_ENABLE_MASK) & ~(MPFS_MSTIMER_MODE_MASK), + MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1CONTROL_OFFSET); + + /* enable interrupts */ + up_enable_irq(MPFS_IRQ_TIMER1); + } +} + +/** + * Handle the timer interrupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context, void *arg) +{ + uint32_t status; + + status = getreg32(MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1RIS_OFFSET); + + /* was this a timer tick? */ + if (status & MPFS_MSTIMER_RIS_MASK) { + + /* 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(); + + /* clear the interrupt */ + putreg32((MPFS_MSTIMER_RIS_MASK), MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1RIS_OFFSET); + } + + 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) +{ + return getreg64(MPFS_CLINT_MTIME); +} + +/** + * Compare a time value with the current time as atomic operation + */ +hrt_abstime +hrt_elapsed_time_atomic(const volatile hrt_abstime *then) +{ + irqstate_t flags = px4_enter_critical_section(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + px4_leave_critical_section(flags); + + return delta; +} + +/** + * Store the absolute time in an interrupt-safe fashion + */ +void +hrt_store_absolute_time(volatile hrt_abstime *t) +{ + irqstate_t flags = px4_enter_critical_section(); + *t = hrt_absolute_time(); + px4_leave_critical_section(flags); +} + +/** + * Initialise 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 = px4_enter_critical_section(); + + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + 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); + + px4_leave_critical_section(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 = px4_enter_critical_section(); + + 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; + + px4_leave_critical_section(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); + hrtinfo("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)) { + hrtinfo("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + hrtinfo("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); + hrtinfo("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) { + hrtinfo("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) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + 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); + uint32_t deadline = HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = HRT_INTERVAL_MIN; + + } else if (next->deadline < now + HRT_INTERVAL_MAX) { + hrtinfo("due soon\n"); + /* calculate how much time we have to the next deadline */ + deadline = (next->deadline - now); + } + } + + hrtinfo("schedule for %"PRIu64" at %"PRIu64"\n", deadline, now); + + /* set next deadline */ + hrt_set_new_deadline(deadline); +} + +static void +hrt_latency_update(void) +{ + uint32_t latency = hrt_absolute_time() - loadval; + 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]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/adc.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/adc.h new file mode 100644 index 000000000000..a08361bbf0be --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/adc.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ +#pragma once + +#define SYSTEM_ADC_BASE 0 // not used diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/hw_description.h new file mode 100644 index 000000000000..948195e753d6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/hw_description.h @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + + +#include + +#include + +#include + +/* + * Timers + */ + +namespace Timer +{ +enum Timer { + FTM0 = 0, +}; +enum Channel { + Channel0 = 0, + Channel1, + Channel2, + Channel3, + Channel4, + Channel5, +}; +struct TimerChannel { + Timer timer; + Channel channel; +}; +} + +namespace GPIO +{ +enum Bank { + BankInvalid = 0, + Bank0, + Bank1, + Bank2, +}; +enum Pin { + Pin0 = 0, + Pin1, + Pin2, + Pin3, + Pin4, + Pin5, + Pin6, + Pin7, + Pin8, + Pin9, + Pin10, + Pin11, + Pin12, + Pin13, + Pin14, + Pin15, + Pin16, + Pin17, + Pin18, + Pin19, + Pin20, + Pin21, + Pin22, + Pin23, + Pin24, + Pin25, + Pin26, + Pin27, + Pin28, + Pin29, + Pin30, + Pin31, +}; + +struct GPIOPin { + Bank bank; + Pin pin; +}; +} + +static inline constexpr uint32_t getGPIOBank(GPIO::Bank bank) +{ + switch (bank) { + case GPIO::Bank0: return GPIO_BANK0; + + case GPIO::Bank1: return GPIO_BANK1; + + case GPIO::Bank2: return GPIO_BANK2; + + default: break; + } + + return 0; +} + +static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin) +{ + switch (pin) { + case GPIO::Pin0: return GPIO_PIN0; + + case GPIO::Pin1: return GPIO_PIN1; + + case GPIO::Pin2: return GPIO_PIN2; + + case GPIO::Pin3: return GPIO_PIN3; + + case GPIO::Pin4: return GPIO_PIN4; + + case GPIO::Pin5: return GPIO_PIN5; + + case GPIO::Pin6: return GPIO_PIN6; + + case GPIO::Pin7: return GPIO_PIN7; + + case GPIO::Pin8: return GPIO_PIN8; + + case GPIO::Pin9: return GPIO_PIN9; + + case GPIO::Pin10: return GPIO_PIN10; + + case GPIO::Pin11: return GPIO_PIN11; + + case GPIO::Pin12: return GPIO_PIN12; + + case GPIO::Pin13: return GPIO_PIN13; + + case GPIO::Pin14: return GPIO_PIN14; + + case GPIO::Pin15: return GPIO_PIN15; + + case GPIO::Pin16: return GPIO_PIN16; + + case GPIO::Pin17: return GPIO_PIN17; + + case GPIO::Pin18: return GPIO_PIN18; + + case GPIO::Pin19: return GPIO_PIN19; + + case GPIO::Pin20: return GPIO_PIN20; + + case GPIO::Pin21: return GPIO_PIN21; + + case GPIO::Pin22: return GPIO_PIN22; + + case GPIO::Pin23: return GPIO_PIN23; + + case GPIO::Pin24: return GPIO_PIN24; + + case GPIO::Pin25: return GPIO_PIN25; + + case GPIO::Pin26: return GPIO_PIN26; + + case GPIO::Pin27: return GPIO_PIN27; + + case GPIO::Pin28: return GPIO_PIN28; + + case GPIO::Pin29: return GPIO_PIN29; + + case GPIO::Pin30: return GPIO_PIN30; + + case GPIO::Pin31: return GPIO_PIN31; + } + + return 0; +} + + +namespace SPI +{ +enum class Bus { + SPI0 = 1, + SPI1 = 2, +}; + +using CS = GPIO::GPIOPin; ///< chip-select pin +using DRDY = GPIO::GPIOPin; ///< data ready pin + +struct bus_device_external_cfg_t { + CS cs_gpio; + DRDY drdy_gpio; +}; + +} // namespace SPI diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..cc225c6ca4b4 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ +#pragma once + +#include "../../../microchip_common/include/px4_arch/i2c_hw_description.h" diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..a7d9d1bb4fed --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/micro_hal.h @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ +#pragma once + + +#include "../../../microchip_common/include/px4_arch/micro_hal.h" + +__BEGIN_DECLS + +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_MPFS + +#define PX4_NUMBER_I2C_BUSES 2 + +#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE +#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO + +#include +#include +#include +#include + +#include "riscv_mmu.h" + +/* + * PX4 uses the words in bigendian order MSB to LSB + * word [0] [1] [2] [3] + * bits 127:96 95-64 63-32, 31-00, + */ +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* Battery backed up SRAM definitions. TODO: check what memory can actually be used */ +#define PX4_BBSRAM_SIZE 2048 + + +// TODO: Use some proper UUID which can be obtained from the HW +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* define common formating across all commands */ + +#define PX4_CPU_UUID_WORD32_FORMAT "%08x" +#define PX4_CPU_UUID_WORD32_SEPARATOR ":" + +#define PX4_CPU_UUID_WORD32_UNIQUE_H 3 /* Least significant digits change the most */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 2 /* Middle High significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_L 1 /* Middle Low significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_N 0 /* Most significant digits change the least */ + +#define PX4_BUS_OFFSET 1 /* MPFS buses are 0 based, so adjustment needed */ +#define px4_savepanic(fileno, context, length) mpfs_bbsram_savepanic(fileno, context, length) +#define px4_spibus_initialize(bus_num_1based) mpfs_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) mpfs_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) mpfs_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) mpfs_configgpio(pinset) +#define px4_arch_unconfiggpio(pinset) mpfs_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) mpfs_gpioread(pinset) +#define px4_arch_gpiowrite(pinset, value) mpfs_gpiowrite(pinset, value) +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) mpfs_gpiosetevent(pinset,r,f,false,false,e,fp,a) + +#define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (~GPIO_OUTPUT | GPIO_BANK_MASK | GPIO_PIN_MASK | GPIO_BUFFER_MASK)) | GPIO_INPUT) + +#define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (~GPIO_INPUT | GPIO_BANK_MASK | GPIO_PIN_MASK | GPIO_BUFFER_MASK)) | GPIO_OUTPUT) + +#define PX4_GPIO_PIN_OFF(gpio) (gpio & (GPIO_BANK_MASK | GPIO_PIN_MASK)) + +/* MPFS can access HRT timer counter directly from user space in + * CONFIG_BUILD_PROTECTED and CONFIG_BUILD_KERNEL + */ + +#define PX4_USERSPACE_HRT +static inline uintptr_t hrt_absolute_time_usr_base(void) +{ + return USRIO_START + (MPFS_CLINT_MTIME & RV_MMU_PAGE_MASK); +} + +// TODO! +# define px4_cache_aligned_data() +# define px4_cache_aligned_alloc malloc + +__END_DECLS diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..94fe842fa54c --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/spi_hw_description.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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. + * + ****************************************************************************/ +#pragma once + +#include "../../../microchip_common/include/px4_arch/spi_hw_description.h" + +#if defined(CONFIG_SPI) + +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +{ + const bool nuttx_enabled_spi_buses[] = { + +#ifdef CONFIG_MPFS_SPI0 + true, +#else + false, +#endif +#ifdef CONFIG_MPFS_SPI1 + true, +#else + false, +#endif + + }; + + for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) { + bool found_bus = false; + + for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) { + if (spi_busses_conf[j].bus == (int)i + 1) { + found_bus = true; + } + } + + // Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured + constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_MPFS_SPIx)"); + } + + return false; +} + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/CMakeLists.txt new file mode 100644 index 000000000000..74be8ec1bb43 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_tone_alarm + ToneAlarmInterface.cpp +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/ToneAlarmInterface.cpp new file mode 100644 index 000000000000..7b14e1cecbed --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/ToneAlarmInterface.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 ToneAlarmInterface.cpp + */ + +#include +#include +#include +#include +#include +#include + +#ifndef TONE_ALARM_PWM_OUT_PATH +#define TONE_ALARM_PWM_OUT_PATH "/dev/pwmX"; +#endif + +#define TONE_ALARM_PWM_DUTY 50 + +namespace ToneAlarmInterface +{ + +int pwm_fd = 0; + +void init() +{ + /* Open the PWM devnode */ + pwm_fd = open(TONE_ALARM_PWM_OUT_PATH, O_RDONLY); + + if (pwm_fd < 0) { + PX4_ERR("failed to open file %s\n", TONE_ALARM_PWM_OUT_PATH); + } +} + +hrt_abstime start_note(unsigned frequency) +{ + hrt_abstime time_started = 0; + + struct pwm_info_s pwm; + memset(&pwm, 0, sizeof(struct pwm_info_s)); + + pwm.frequency = frequency; + pwm.channels[0].channel = 1; + pwm.channels[0].duty = ((uint32_t)(TONE_ALARM_PWM_DUTY << 16) / 100); + + irqstate_t flags = enter_critical_section(); + + // Set frequency + if (ioctl(pwm_fd, PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)) < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS) failed: %d\n", errno); + } + + time_started = hrt_absolute_time(); + + // Start + if (ioctl(pwm_fd, PWMIOC_START, 0) < 0) { + PX4_ERR("PWMIOC_START failed: %d\n", errno); + } + + leave_critical_section(flags); + + return time_started; +} + +void stop_note() +{ + if (ioctl(pwm_fd, PWMIOC_STOP, 0) < 0) { + PX4_ERR("PWMIOC_STOP failed: %d\n", errno); + } +} + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/microchip/mpfs/version/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/version/CMakeLists.txt new file mode 100644 index 000000000000..3c25950736b6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/version/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ + +px4_add_library(arch_version + board_mcu_version.c +) + +if (CONFIG_BUILD_FLAT) + target_link_libraries(arch_version PRIVATE nuttx_arch) +else() + target_link_libraries(arch_version PRIVATE nuttx_karch) +endif() diff --git a/platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c b/platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c new file mode 100644 index 000000000000..ec075994ed67 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. 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 usr_mcu_version.c + * Implementation of MPFS version API + */ + +#include +#include +#include +#include + +#include +#include +#include + +#ifndef arraySize +#define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) +#endif + +#define HW_INFO_SIZE (int) arraySize(HW_INFO_INIT_PREFIX) + HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS +#define MPFS_SYS_SERVICE_CR 0x37020050 +#define MPFS_SYS_SERVICE_SR 0x37020054 +#define MPFS_SYS_SERVICE_MAILBOX 0x37020800 +#define SERVICE_CR_REQ (1 << 0) +#define SERVICE_SR_BUSY (1 << 1) + +#define getreg8(a) (*(volatile uint8_t *)(a)) +#define getreg32(a) (*(volatile uint32_t *)(a)) +#define putreg32(v,a) (*(volatile uint32_t *)(a) = (v)) + +static int hw_version = 0; +static int hw_revision = 0; +static char hw_info[HW_INFO_SIZE] = {0}; + +static mfguid_t device_serial_number = { 0 }; + +static void determine_hw(void); + +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID_MPFS; + +__EXPORT const char *board_get_hw_type_name(void) +{ + return (const char *) hw_info; +} + +__EXPORT int board_get_hw_version(void) +{ + return hw_version; +} + +__EXPORT int board_get_hw_revision() +{ + return hw_revision; +} + +__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) +{ + + /* DEPRECATED. Use board_get_px4_guid() */ + + uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH]; + memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4); + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uuid_words[i] = chip_uuid[i]; + } +} + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + const struct { + const char *revstr; + char rev; + const char *errata; + } hw_version_table[] = BOARD_REVISIONS; + + int len = sizeof(hw_version_table) / sizeof(hw_version_table[0]); + + if (hw_version < len) { + *rev = hw_version_table[hw_version].rev; + *revstr = hw_version_table[hw_version].revstr; + *errata = hw_version_table[hw_version].errata; + } + + return hw_version; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + + memset(pb, 0, sizeof(px4_guid_t)); + + static_assert(sizeof(device_serial_number) == MPFS_DSN_LENGTH); + static_assert(sizeof(px4_guid_t) >= sizeof(device_serial_number) + 2); + + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + + memcpy(pb, device_serial_number, sizeof(device_serial_number)); + + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} + +/************************************************************************************ + * Name: board_determine_hw_info + * + * Description: + * Uses the HW revision and version detection + * + * Input Parameters: + * None + * + * Returned Value: + * 0 - on success or negated errono + * 1) The values for integer value of this boards hardware revision is set + * 2) The integer value of this boards hardware version is set. + * 3) hw_info is populated + * + ************************************************************************************/ +int board_determine_hw_info(void) +{ + determine_hw(); + + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); + + return OK; +} + +void determine_hw(void) +{ + uint8_t pin1, pin2, pin3; + + /* read device serial number */ + mpfs_read_dsn(device_serial_number, sizeof(device_serial_number)); + + /* determine hw version number */ + px4_arch_configgpio(GPIO_HW_VERSION_PIN1); + px4_arch_configgpio(GPIO_HW_VERSION_PIN2); + px4_arch_configgpio(GPIO_HW_VERSION_PIN3); + + /* wait pins to set */ + usleep(5); + + pin1 = px4_arch_gpioread(GPIO_HW_VERSION_PIN1); + pin2 = px4_arch_gpioread(GPIO_HW_VERSION_PIN2); + pin3 = px4_arch_gpioread(GPIO_HW_VERSION_PIN3); + + hw_version = (pin3 << 2) | (pin2 << 1) | pin1; + +#ifdef BOARD_HAS_MULTIPURPOSE_VERSION_PINS + /* NOTE: utilizes same GPIOs as LEDs. Restore GPIO pin configuration */ + px4_arch_configgpio(GPIO_nLED_RED); + px4_arch_configgpio(GPIO_nLED_GREEN); + px4_arch_configgpio(GPIO_nLED_BLUE); +#endif + +} + +int board_get_mfguid(mfguid_t mfgid) +{ + return mpfs_read_dsn(mfgid, sizeof(mfguid_t)); +} diff --git a/platforms/nuttx/toc/CMakeLists.txt b/platforms/nuttx/toc/CMakeLists.txt new file mode 100644 index 000000000000..646c3835b8b9 --- /dev/null +++ b/platforms/nuttx/toc/CMakeLists.txt @@ -0,0 +1,72 @@ +############################################################################ +# +# Copyright (c) 2022 Technology Innovation Institute. 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. +# +############################################################################ + +add_executable(toc + fw_image.c +) + +set(TOC_NAME ${PX4_BINARY_DIR}/toc.elf) +set_target_properties(toc PROPERTIES OUTPUT_NAME ${TOC_NAME}) +target_compile_options(toc PRIVATE -DPX4_UNSIGNED_FIRMWARE=${PX4_BINARY_OUTPUT}) +set_source_files_properties(fw_image.c PROPERTIES OBJECT_DEPENDS px4_binary) + +add_library(board_toc + ${PX4_BOARD_DIR}/src/toc.c # The board specific ToC file +) +add_dependencies(board_toc nuttx_context) + +target_link_libraries(toc PRIVATE + + -nostartfiles + -nodefaultlibs + -nostdlib + + -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}toc.ld + -Wl,--warn-common + -Wl,-Map=${PX4_BINARY_DIR}/toc.map +) + +if(NOT USE_LD_GOLD) + target_link_libraries(toc PRIVATE -Wl,--print-memory-usage) +endif() + +target_link_libraries(toc PRIVATE board_toc) +target_link_libraries(drivers_board PRIVATE board_toc) + +set(TOC_BINARY_OUTPUT ${PX4_BINARY_DIR}/toc.bin) + +add_custom_command(OUTPUT ${TOC_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${TOC_NAME} ${TOC_BINARY_OUTPUT} + DEPENDS toc +) +add_custom_target(toc_bin DEPENDS ${TOC_BINARY_OUTPUT}) diff --git a/platforms/nuttx/toc/fw_image.c b/platforms/nuttx/toc/fw_image.c new file mode 100644 index 000000000000..2ea88b9d6652 --- /dev/null +++ b/platforms/nuttx/toc/fw_image.c @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#ifndef PX4_UNSIGNED_FIRMWARE +# error "The path to the unsigned PX4 image is not set" +#endif + +/* The unsigned firmware is injected here + * + * The build process goes as follows: + * 1. The unsigned firmware is built + * - In case of protected mode the kernel + user bin files are concatenated + * together (with padding), resulting in a single, unsigned binary file + * 2. The Table-of-Contents (ToC) is built along with this file. This provides + * the ToC with the firmware boundaries and the location of the firmware + * signature. + * 3. The resulting ToC + firmware binary is signed, which is the final result + */ + +#define __STR(s) #s +#define __XSTR(s) __STR(s) + +__asm__ +( + ".section .firmware,\"ax\"\n" + ".incbin \"" __XSTR(PX4_UNSIGNED_FIRMWARE) "\"\n" +); diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index 8baf5570e137..71e4eb23f82a 100644 --- a/platforms/posix/src/px4/common/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -79,6 +79,8 @@ #include "px4_daemon/server.h" #include "px4_daemon/pxh.h" +#include + #define MODULE_NAME "px4" static const char *LOCK_FILE_PATH = "/tmp/px4_lock"; diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index c03e63aca45a..4fcbf0aa493f 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -852,7 +852,7 @@ CameraTrigger::engage(void *arg) trigger.feedback = false; trigger.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); + orb_publish(ORB_ID(camera_trigger), &trig->_trigger_pub, &trigger); // increment frame count trig->_trigger_seq++; diff --git a/src/drivers/cyphal/legacy_data_types b/src/drivers/cyphal/legacy_data_types deleted file mode 160000 index 36a01e428b11..000000000000 --- a/src/drivers/cyphal/legacy_data_types +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index 3144237ab899..db314c1f47ed 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -196,7 +196,7 @@ class MappyDot : public device::I2C, public ModuleParams, public I2CSPIDriver +#include #endif __BEGIN_DECLS @@ -81,12 +82,10 @@ typedef struct hrt_call { hrt_callout callout; void *arg; #if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) - hrt_callout usr_callout; - void *usr_arg; + px4_sem_t *callout_sem; #endif } *hrt_call_t; - #define LATENCY_BUCKET_COUNT 8 extern const uint16_t latency_bucket_count; extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT]; @@ -99,7 +98,10 @@ typedef struct latency_info { #if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +typedef void *px4_hrt_handle_t; + typedef struct hrt_boardctl { + const px4_hrt_handle_t handle; hrt_call_t entry; hrt_abstime time; /* delay or calltime */ hrt_abstime interval; @@ -123,6 +125,9 @@ typedef struct latency_boardctl { #define HRT_CANCEL _HRTIOC(6) #define HRT_GET_LATENCY _HRTIOC(7) #define HRT_RESET_LATENCY _HRTIOC(8) +#define HRT_REGISTER _HRTIOC(9) +#define HRT_UNREGISTER _HRTIOC(10) +#define HRT_ABSTIME_BASE _HRTIOC(11) #endif diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp index a28f6f270654..0c17bdfd4d43 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include using namespace Analog_Devices_ADIS16448; @@ -114,7 +114,7 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp index 40c6fab14f9f..a0a94056295f 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace Analog_Devices_ADIS16470; @@ -107,7 +107,7 @@ class ADIS16470 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; bool _self_test_passed{false}; diff --git a/src/drivers/imu/bosch/bmi055/BMI055.hpp b/src/drivers/imu/bosch/bmi055/BMI055.hpp index f4fad2772ea2..6cc8617cb5b7 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -66,7 +67,7 @@ class BMI055 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp index 35187fdb7203..2167c61aa2f1 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -66,7 +67,7 @@ class BMI088 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp index 6c1734a059db..25068c1c9245 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -72,7 +73,7 @@ class BMI088 : public device::I2C, public I2CSPIDriver int _total_failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index 3855ce12ea93..8838cd48c34e 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20602; @@ -140,7 +140,7 @@ class ICM20602 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index 9608eeb816ac..bd11f45bcf4d 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20608G; @@ -138,7 +138,7 @@ class ICM20608G : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index 531f0be988ef..43aaa1c632c7 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20649; @@ -152,7 +152,7 @@ class ICM20649 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index df3b8a734e90..206b57ed5cff 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20689; @@ -138,7 +138,7 @@ class ICM20689 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 103a03fe573e..c9e53deab785 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include "ICM20948_AK09916.hpp" @@ -169,7 +169,7 @@ class ICM20948 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index ab6fcfa1f27b..f3f47677539e 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM40609D; @@ -146,7 +146,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp index 98ad916d90dc..d4bee687721c 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42605; @@ -146,7 +146,7 @@ class ICM42605 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp index 362418f7aea3..ec497d67ea72 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42670P; @@ -150,7 +150,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index dea93d9524d7..01ca68a6e360 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -149,6 +149,9 @@ void ICM42688P::RunImpl() && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00) && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { + // Disable I2C interface + RegisterSetAndClearBits(Register::BANK_0::INTF_CONFIG0, 0x3, 0); + // Wakeup accel and gyro and schedule remaining configuration RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE); _state = STATE::CONFIGURE; diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index b118f8c92065..c2ba6cd72326 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42688P; @@ -158,7 +158,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 2d5180013a22..ff8df4f409d7 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU6000; @@ -140,7 +140,7 @@ class MPU6000 : public device::SPI, public I2CSPIDriver FIFO::DATA _fifo_sample_last_new_accel{}; uint32_t _fifo_accel_samples_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 7686db5114e5..712790ec3266 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU6500; @@ -138,7 +138,7 @@ class MPU6500 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 369d2ec6e7cb..c0d26521d21b 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include "MPU9250_AK8963.hpp" @@ -150,7 +150,7 @@ class MPU9250 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp index 7af10d0b6076..d6aa3bdde105 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU9250; @@ -136,7 +136,7 @@ class MPU9250_I2C : public device::I2C, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index 588c624c4476..0130b974a130 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -138,7 +138,7 @@ class PCA9685 : public device::I2C, public I2CSPIDriver uint8_t _msg[6]; - int _actuator_controls_sub; + orb_sub_t _actuator_controls_sub; struct actuator_controls_s _actuator_controls; uint16_t _current_values[actuator_controls_s::NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output values as sent to the setPin() */ @@ -181,7 +181,7 @@ PCA9685::PCA9685(const I2CSPIDriverConfig &config) : _mode(IOX_MODE_ON), _i2cpwm_interval(1_s / 60.0f), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), - _actuator_controls_sub(-1), + _actuator_controls_sub(ORB_SUB_INVALID), _actuator_controls(), _mode_on_initialized(false) { diff --git a/src/drivers/pfsoc_crypto b/src/drivers/pfsoc_crypto new file mode 160000 index 000000000000..5a800f4070bb --- /dev/null +++ b/src/drivers/pfsoc_crypto @@ -0,0 +1 @@ +Subproject commit 5a800f4070bb0cf596536e73b8b0ae146fe4e9de diff --git a/src/drivers/pfsoc_keystore b/src/drivers/pfsoc_keystore new file mode 160000 index 000000000000..e62e6a91c068 --- /dev/null +++ b/src/drivers/pfsoc_keystore @@ -0,0 +1 @@ +Subproject commit e62e6a91c068630f75e5e9b9734f9c1443001497 diff --git a/src/drivers/pwm_esc/CMakeLists.txt b/src/drivers/pwm_esc/CMakeLists.txt new file mode 100644 index 000000000000..d0d8260cf64c --- /dev/null +++ b/src/drivers/pwm_esc/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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. +# +############################################################################ +px4_add_module( + MODULE drivers__pwm_esc + MAIN pwm_esc + COMPILE_FLAGS + SRCS + pwm_esc.cpp + MODULE_CONFIG + module.yaml + DEPENDS + mixer_module + ) diff --git a/src/drivers/pwm_esc/Kconfig b/src/drivers/pwm_esc/Kconfig new file mode 100644 index 000000000000..04a3ff1b0f97 --- /dev/null +++ b/src/drivers/pwm_esc/Kconfig @@ -0,0 +1,12 @@ +menuconfig DRIVERS_PWM_ESC + bool "pwm_esc" + default n + ---help--- + Enable support for pwm_esc + +menuconfig USER_PWM_ESC + bool "pwm_esc running as userspace module" + default n + depends on BOARD_PROTECTED && DRIVERS_PWM_ESC + ---help--- + Put pwm_esc in userspace memory diff --git a/src/drivers/pwm_esc/module.yaml b/src/drivers/pwm_esc/module.yaml new file mode 100644 index 000000000000..d541dd1a710e --- /dev/null +++ b/src/drivers/pwm_esc/module.yaml @@ -0,0 +1,12 @@ +module_name: PWMESC +actuator_output: + output_groups: + - param_prefix: PWM_MAIN + channel_label: 'MAIN' + channel_label_module_name_prefix: false + num_channels: 10 + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } diff --git a/src/drivers/pwm_esc/pwm_esc.cpp b/src/drivers/pwm_esc/pwm_esc.cpp new file mode 100644 index 000000000000..8d9a918ab973 --- /dev/null +++ b/src/drivers/pwm_esc/pwm_esc.cpp @@ -0,0 +1,554 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 pwm_esc.cpp + * Driver for the NuttX PWM driver controleed escs + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#ifndef PWMESC_OUT_PATH +#define PWMESC_OUT_PATH "/dev/pwmX"; +#endif + +#ifndef PWMESC_MAX_DEVICES +#define PWMESC_MAX_DEVICES 2 +#endif + +#ifndef PWMESC_MAX_CHANNELS +#define PWMESC_MAX_CHANNELS 10 +#endif + +#ifndef PWM_DEFAULT_RATE +#define PWM_DEFAULT_RATE 400 +#endif + +//using namespace time_literals; + +/** + * The PWMESC class. + * + */ +class PWMESC : public OutputModuleInterface +{ +public: + /** + * Constructor. + * + * Initialize all class variables. + */ + PWMESC(); + + /** + * Destructor. + * + * Wait for worker thread to terminate. + */ + virtual ~PWMESC(); + + /** + * Initialize the PWMESC class. + * + * Retrieve relevant initial system parameters. Connect to PWM device + * + * @param hitl_mode set to suppress publication of actuator_outputs + */ + int init(bool hitl_mode); + + /** + * Start the PWMESC driver + */ + static int start(int argc, char *argv[]); + + /** + * Stop the PWMESC driver + */ + static int stop(); + + /** + * Return if the PWMESC driver is already running + */ + bool running() {return _initialized;}; + + /** + * updateOutputs + * + * Sets the actual PWM outputs. See OutputModuleInterface + * + */ + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + /** + * Don't allow more channels than MAX_ACTUATORS + */ + static_assert(PWMESC_MAX_CHANNELS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails"); + +private: + + bool _initialized{false}; + + volatile int _task; ///< worker task id + volatile bool _task_should_exit; ///< worker terminate flag + + px4_sem_t _update_sem; + + perf_counter_t _perf_update; ///< local performance counter for PWM updates + + /* subscribed topics */ + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + + MixingOutput _mixing_output{"PWM_MAIN", PWMESC_MAX_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1000000}; + + /* advertised topics */ + uORB::PublicationMulti _actuator_outputs_pub{ORB_ID(actuator_outputs)}; + + actuator_armed_s _actuator_armed; + + bool _hitl_mode; ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs + + int _pwm_fd[PWMESC_MAX_DEVICES]; + + int32_t _pwm_rate{PWM_DEFAULT_RATE}; + + int init_pwm_outputs(); + + /* Singleton pointer */ + static PWMESC *_instance; + + /** + * Trampoline to the worker task + */ + static int task_main_trampoline(int argc, char *argv[]); + + /** + * worker task + */ + void task_main(); + + /** + * Callback for mixer subscriptions + */ + void Run() override; + + void update_params(); + + /* No copy constructor */ + PWMESC(const PWMESC &); + PWMESC operator=(const PWMESC &); + + /** + * Get the singleton instance + */ + static inline PWMESC *getInstance() + { + if (_instance == nullptr) { + /* create the driver */ + _instance = new PWMESC(); + } + + return _instance; + } + + /** + * Set the PWMs on open device fd to 0 duty cycle + * and start or stop (request == PWMIOC_START / PWMIOC_STOP) + */ + int set_defaults(int fd, unsigned long request); +}; + +PWMESC *PWMESC::_instance = nullptr; + +PWMESC::PWMESC() : + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _task(-1), + _task_should_exit(false), + _perf_update(perf_alloc(PC_ELAPSED, "pwm update")), + _hitl_mode(false) +{ + + /* initialize tick semaphores */ + px4_sem_init(&_update_sem, 0, 0); + px4_sem_setprotocol(&_update_sem, SEM_PRIO_NONE); + + /* clear armed status */ + memset(&_actuator_armed, 0, sizeof(actuator_armed_s)); +} + +PWMESC::~PWMESC() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + px4_usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) { + PX4_ERR("Task exit fail\n"); + px4_task_delete(_task); + } + + /* deallocate perfs */ + perf_free(_perf_update); + + px4_sem_destroy(&_update_sem); +} + +int +PWMESC::init(bool hitl_mode) +{ + _hitl_mode = hitl_mode; + + /* start the main task */ + _task = px4_task_spawn_cmd("pwm_esc", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 3048, + (px4_main_t)&PWMESC::task_main_trampoline, + nullptr); + + if (_task < 0) { + PX4_ERR("task start failed: %d", errno); + return -errno; + } + + _initialized = true; + + /* schedule workqueue */ + ScheduleNow(); + + return OK; +} + +int +PWMESC::task_main_trampoline(int argc, char *argv[]) +{ + getInstance()->task_main(); + return 0; +} + +bool +PWMESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + bool ret = true; + struct pwm_info_s pwm; + + memset(&pwm, 0, sizeof(struct pwm_info_s)); + pwm.frequency = _pwm_rate; + + for (unsigned i = 0; i < num_outputs; i++) { + // TODO: channel to proper pwm device map. + // this is now just quick hack for one pwm device, direct map of channels + + uint16_t pwm_val = outputs[i]; + pwm.channels[i].duty = ((((uint32_t)pwm_val) << 16) / (1000000 / _pwm_rate)); + pwm.channels[i].channel = i + 1; + } + + /* Only publish if not in hitl */ + + if (!_hitl_mode && + ::ioctl(_pwm_fd[0], PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)) < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n", + errno); + ret = false; + } + + return ret; +} + +int +PWMESC::set_defaults(int fd, unsigned long request) +{ + /* Configure PWM to default rate, 1 as duty and start */ + + struct pwm_info_s pwm; + memset(&pwm, 0, sizeof(struct pwm_info_s)); + pwm.frequency = _pwm_rate; + + for (int j = 0; j < PWMESC_MAX_CHANNELS; j++) { + pwm.channels[j].duty = 1; /* 0 is not allowed duty cycle value */ + pwm.channels[j].channel = j + 1; + } + + /* Set the frequency and duty */ + + int ret = ::ioctl(fd, PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)); + + if (ret < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS) failed: %d\n", + errno); + + } else { + + /* Start / stop */ + + ret = ::ioctl(fd, request, 0); + + if (ret < 0) { + PX4_ERR("PWMIOC_START/STOP failed: %d\n", errno); + } + + } + + return ret; +} + +int +PWMESC::init_pwm_outputs() +{ + int ret = 0; + + _mixing_output.setIgnoreLockdown(_hitl_mode); + _mixing_output.setMaxNumOutputs(PWMESC_MAX_CHANNELS); + const int update_interval_in_us = math::constrain(1000000 / (_pwm_rate * 2), 500, 100000); + _mixing_output.setMaxTopicUpdateRate(update_interval_in_us); + + /* Open the PWM devnode */ + + // TODO: loop through the devices, open all fd:s + char pwm_device_name[] = PWMESC_OUT_PATH; + int n_pwm_devices = 1; + + for (int i = 0; i < 1 && i < n_pwm_devices; i++) { + pwm_device_name[sizeof(pwm_device_name) - 2] = '0' + i; + + _pwm_fd[i] = ::open(pwm_device_name, O_RDONLY); + + if (_pwm_fd[i] < 0) { + PX4_ERR("pwm_main: open %s failed: %d\n", pwm_device_name, errno); + ret = -1; + continue; + } + + /* Configure PWM to default rate, 0 pulse and start */ + + if (set_defaults(_pwm_fd[i], PWMIOC_START) != 0) { + ret = -1; + } + } + + return ret; +} + +void +PWMESC::Run() +{ + /* Just trigger the main task */ + px4_sem_post(&_update_sem); +} + +void +PWMESC::task_main() +{ + if (init_pwm_outputs() != 0) { + PX4_ERR("PWM initialization failed"); + _task_should_exit = true; + } + + while (!_task_should_exit) { + + /* Get the armed status */ + + _actuator_armed_sub.update(&_actuator_armed); + + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + /* Add 100 ms, this can't overflow */ + ts.tv_nsec += 100000000; + + if (ts.tv_nsec >= 1000000000) { + ts.tv_nsec -= 1000000000; + ts.tv_sec += 1; + } + + /* sleep waiting for mixer update */ + int ret = px4_sem_timedwait(&_update_sem, &ts); + + perf_begin(_perf_update); + + if (ret == 0) { + _mixing_output.update(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + update_params(); + } + + _mixing_output.updateSubscriptions(true); + + perf_end(_perf_update); + } + + PX4_DEBUG("exiting"); + + /* Configure PWM to default rate, 0 pulse and stop */ + + int n_pwm_devices = 1; + + for (int i = 0; i < 1 && i < n_pwm_devices; i++) { + set_defaults(_pwm_fd[i], PWMIOC_STOP); + } + + /* tell the dtor that we are exiting */ + _task = -1; +} + +void PWMESC::update_params() +{ + /* skip update when armed */ + if (_actuator_armed.armed) { + return; + } + + /* Call MixingOutput::updateParams */ + updateParams(); +} + +extern "C" __EXPORT int pwm_esc_main(int argc, char *argv[]); + +int +PWMESC::start(int argc, char *argv[]) +{ + int ret = 0; + + if (PWMESC::getInstance() == nullptr) { + PX4_ERR("Driver allocation failed"); + return -1; + } + + if (PWMESC::getInstance()->running()) { + PX4_ERR("Already running"); + return -1; + } + + bool hitl_mode = false; + + /* Check if started in hil mode */ + for (int extra_args = 1; extra_args < argc; extra_args++) { + if (!strcmp(argv[extra_args], "hil")) { + hitl_mode = true; + + } else if (argv[extra_args][0] != '\0') { + PX4_WARN("unknown argument: %s", argv[extra_args]); + } + } + + if (OK != PWMESC::getInstance()->init(hitl_mode)) { + delete PWMESC::getInstance(); + PX4_ERR("Driver init failed"); + return -1; + } + + return ret; +} + +int +PWMESC::stop() +{ + if (PWMESC::getInstance() == nullptr) { + PX4_ERR("Driver allocation failed"); + return -1; + } + + if (!PWMESC::getInstance()->running()) { + PX4_ERR("Not running"); + return -1; + } + + delete (PWMESC::getInstance()); + _instance = nullptr; + + return 0; +} + +int +pwm_esc_main(int argc, char *argv[]) +{ + /* check for sufficient number of arguments */ + if (argc < 2) { + PX4_ERR("Need a command, try 'start' / 'stop'"); + return -1; + } + + if (!strcmp(argv[1], "start")) { + return PWMESC::start(argc - 1, argv + 1); + } + + if (!strcmp(argv[1], "stop")) { + return PWMESC::stop(); + } + + return 0; +} diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 70d8ec600440..1550384c8264 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -45,7 +45,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 4010e0d5133e..17ae86a507f5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -55,7 +55,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include "uploader.h" diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 89c2ee783e7c..7580744b3851 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -79,7 +79,7 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): _uart(0), _uart_set(), _uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US}, - _actuatorsSub(-1), + _actuatorsSub(ORB_SUB_INVALID), _lastEncoderCount{0, 0}, _encoderCounts{0, 0}, _motorSpeeds{0, 0} @@ -160,7 +160,7 @@ void RoboClaw::taskMain() _armedSub = orb_subscribe(ORB_ID(actuator_armed)); _paramSub = orb_subscribe(ORB_ID(parameter_update)); - pollfd fds[3]; + px4_pollfd_struct_t fds[3]; fds[0].fd = _paramSub; fds[0].events = POLLIN; fds[1].fd = _actuatorsSub; @@ -174,7 +174,7 @@ void RoboClaw::taskMain() while (!taskShouldExit) { - int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); + int pret = px4_poll(fds, sizeof(fds) / sizeof(px4_pollfd_struct_t), waitTime / 1000); bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms; @@ -564,7 +564,7 @@ void RoboClaw::_parameters_update() param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms); param_get(_param_handles.address, &_parameters.address); - if (_actuatorsSub > 0) { + if (orb_sub_valid(_actuatorsSub)) { orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 0a92d07f3993..a595d2e4338d 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -196,13 +196,13 @@ class RoboClaw struct timeval _uart_timeout; /** actuator controls subscription */ - int _actuatorsSub{-1}; + orb_sub_t _actuatorsSub{ORB_SUB_INVALID}; actuator_controls_s _actuatorControls; - int _armedSub{-1}; + orb_sub_t _armedSub{ORB_SUB_INVALID}; actuator_armed_s _actuatorArmed; - int _paramSub{-1}; + orb_sub_t _paramSub{ORB_SUB_INVALID}; parameter_update_s _paramUpdate; uORB::PublicationMulti _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)}; diff --git a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp index ee39c6d970c2..177426a241a6 100644 --- a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp +++ b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp @@ -146,7 +146,7 @@ void task_main(int argc, char *argv[]) rc_pub = orb_advertise(ORB_ID(input_rc), &input_rc); } else { - orb_publish(ORB_ID(input_rc), rc_pub, &input_rc); + orb_publish(ORB_ID(input_rc), &rc_pub, &input_rc); } } diff --git a/src/drivers/stub_keystore/CMakeLists.txt b/src/drivers/stub_keystore/CMakeLists.txt index ecbac32daa0d..d3f71ed5a863 100644 --- a/src/drivers/stub_keystore/CMakeLists.txt +++ b/src/drivers/stub_keystore/CMakeLists.txt @@ -36,6 +36,10 @@ px4_add_library(keystore_backend stub_keystore.c) target_include_directories(keystore_backend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +# This interface library can be used to get the backend headers +add_library(keystore_backend_interface INTERFACE) +target_include_directories(keystore_backend_interface INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + # Parse keyfile locations from boardconfig # If the key file path is defined in environment variable, use it from there diff --git a/src/drivers/stub_keystore/stub_keystore.c b/src/drivers/stub_keystore/stub_keystore.c index 0f33436479a5..1e5054021ad3 100644 --- a/src/drivers/stub_keystore/stub_keystore.c +++ b/src/drivers/stub_keystore/stub_keystore.c @@ -45,6 +45,10 @@ void keystore_init(void) { } +void keystore_deinit(void) +{ +} + keystore_session_handle_t keystore_open(void) { keystore_session_handle_t ret; diff --git a/src/drivers/sw_crypto/CMakeLists.txt b/src/drivers/sw_crypto/CMakeLists.txt index 16d2d215cc1d..0e4535e33c1a 100644 --- a/src/drivers/sw_crypto/CMakeLists.txt +++ b/src/drivers/sw_crypto/CMakeLists.txt @@ -45,5 +45,9 @@ target_link_libraries(crypto_backend monocypher libtomcrypt ) - target_include_directories(crypto_backend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# This interface library can be used to get the backend headers +add_library(crypto_backend_interface INTERFACE) +target_link_libraries(crypto_backend_interface INTERFACE keystore_backend_interface) +target_include_directories(crypto_backend_interface INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/drivers/sw_crypto/crypto.c b/src/drivers/sw_crypto/crypto.c index 13786b87ab09..7bd8be449112 100644 --- a/src/drivers/sw_crypto/crypto.c +++ b/src/drivers/sw_crypto/crypto.c @@ -59,6 +59,10 @@ extern void libtomcrypt_init(void); #define SECMEM_FREE XFREE #endif +#define SHA256_HASHLEN 32 +#define OAEP_MAX_RSA_MODLEN 256 /* RSA2048 */ +#define OAEP_MAX_MSGLEN (OAEP_MAX_RSA_MODLEN - 2 * SHA256_HASHLEN - 2) + /* * For now, this is just a dummy up/down counter for tracking open/close calls */ @@ -151,6 +155,11 @@ void crypto_init() clear_key_cache(); } +void crypto_deinit() +{ + keystore_deinit(); +} + crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm) { crypto_session_handle_t ret; @@ -225,7 +234,14 @@ bool crypto_signature_check(crypto_session_handle_t handle, switch (handle.algorithm) { case CRYPTO_ED25519: - ret = crypto_ed25519_check(signature, public_key, message, message_size) == 0; + if (keylen >= 32) { + /* In the DER format ed25510 key the raw public key part is always the last 32 bytes. + * This simple "parsing" works for both "raw" key and DER format + */ + public_key += keylen - 32; + ret = crypto_ed25519_check(signature, public_key, message, message_size) == 0; + } + break; default: @@ -378,12 +394,22 @@ bool crypto_get_encrypted_key(crypto_session_handle_t handle, max_len); } else { - // The key size, encrypted, is a multiple of minimum block size for the algorithm+key - size_t min_block = crypto_get_min_blocksize(handle, encryption_key_idx); - *max_len = key_sz / min_block * min_block; + switch (handle.algorithm) { + + case CRYPTO_RSA_OAEP: + /* The length is the RSA key modulus length, and the maximum plaintext + * length is calculated from that. This is now just fixed for RSA2048, + * but one could also parse the RSA key + * (encryption_key_idx) here and calculate the lengths. + */ - if (key_sz % min_block) { - *max_len += min_block; + *max_len = key_sz <= OAEP_MAX_MSGLEN ? OAEP_MAX_RSA_MODLEN : 0; + ret = true; + break; + + default: + *max_len = 0; + break; } } @@ -423,24 +449,6 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx) ret = 64; break; - case CRYPTO_RSA_OAEP: { - rsa_key enc_key; - size_t pub_key_sz; - uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz); - - initialize_tomcrypt(); - - if (pub_key && - rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) { - ret = ltc_mp.unsigned_size(enc_key.N); - rsa_free(&enc_key); - - } else { - ret = 0; - } - } - break; - default: ret = 1; } diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp index 1924b3529fbe..9310e9413a16 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp +++ b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp @@ -396,7 +396,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) float filtered_alt = NAN; float last_baro_alt = 0.f; - int airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); + orb_sub_t airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); uint32_t lastBATV_ms = 0; uint32_t lastCUR_ms = 0; diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 96c2dd338685..55dee09de5d0 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -58,14 +58,14 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 -static int _battery_sub = -1; -static int _gps_sub = -1; -static int _home_sub = -1; -static int _airdata_sub = -1; -static int _airspeed_sub = -1; -static int _esc_sub = -1; +static orb_sub_t _battery_sub = ORB_SUB_INVALID; +static orb_sub_t _gps_sub = ORB_SUB_INVALID; +static orb_sub_t _home_sub = ORB_SUB_INVALID; +static orb_sub_t _airdata_sub = ORB_SUB_INVALID; +static orb_sub_t _airspeed_sub = ORB_SUB_INVALID; +static orb_sub_t _esc_sub = ORB_SUB_INVALID; -static orb_advert_t _esc_pub = nullptr; +static orb_advert_t _esc_pub = ORB_ADVERT_INVALID; static bool _home_position_set = false; static double _home_lat = 0.0d; @@ -122,8 +122,8 @@ publish_gam_message(const uint8_t *buffer) esc.esc[0].esc_current = static_cast((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F; /* announce the esc if needed, just publish else */ - if (_esc_pub != nullptr) { - orb_publish(ORB_ID(esc_status), _esc_pub, &esc); + if (orb_advert_valid(_esc_pub)) { + orb_publish(ORB_ID(esc_status), &_esc_pub, &esc); } else { _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index 5893f9018f7e..77b7d241fc55 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -42,6 +42,8 @@ using namespace time_literals; +bool ToneAlarm::_stop_note = false; + ToneAlarm::ToneAlarm() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { @@ -51,13 +53,11 @@ ToneAlarm::ToneAlarm() : ToneAlarm::~ToneAlarm() { - ToneAlarmInterface::stop_note(); + // stop_note() is handled via request_stop() } bool ToneAlarm::Init() { - // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. - ToneAlarmInterface::init(); _tune_control_sub.set_interval_us(10_ms); @@ -73,11 +73,23 @@ bool ToneAlarm::Init() void ToneAlarm::InterruptStopNote(void *arg) { - ToneAlarmInterface::stop_note(); + _stop_note = true; } void ToneAlarm::Run() { + + if (!_initialized) { + // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. + ToneAlarmInterface::init(); + _initialized = true; + } + + if (_stop_note) { + ToneAlarmInterface::stop_note(); + _stop_note = false; + } + // Check if circuit breaker is enabled. if (!_circuit_break_initialized) { if (circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY)) { @@ -88,6 +100,7 @@ void ToneAlarm::Run() } if (should_exit()) { + ToneAlarmInterface::stop_note(); _tune_control_sub.unregisterCallback(); exit_and_cleanup(); return; @@ -211,8 +224,8 @@ void ToneAlarm::Run() hrt_call_at(&_hrt_call, time_started + duration, (hrt_callout)&InterruptStopNote, this); _next_note_time = time_started + duration + silence_length; - // schedule next note - ScheduleAt(_next_note_time); + // schedule at stop time + ScheduleAt(time_started + duration); } } else { @@ -235,6 +248,12 @@ void ToneAlarm::Run() } } +void ToneAlarm::request_stop() +{ + ModuleBase::request_stop(); + ScheduleNow(); +} + int ToneAlarm::task_spawn(int argc, char *argv[]) { ToneAlarm *instance = new ToneAlarm(); diff --git a/src/drivers/tone_alarm/ToneAlarm.h b/src/drivers/tone_alarm/ToneAlarm.h index fa04993e8c8c..ab1f4a89b49f 100644 --- a/src/drivers/tone_alarm/ToneAlarm.h +++ b/src/drivers/tone_alarm/ToneAlarm.h @@ -70,6 +70,8 @@ class ToneAlarm : public ModuleBase, public px4::ScheduledWorkItem /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + void request_stop() override; + private: static void InterruptStopNote(void *arg); @@ -86,4 +88,6 @@ class ToneAlarm : public ModuleBase, public px4::ScheduledWorkItem bool _circuit_break_initialized{false}; bool _play_tone{false}; + bool _initialized{false}; + static bool _stop_note; }; diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 1ea0213c2f21..98c8fbd83543 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") if(CONFIG_ARCH_CHIP) - if(${CONFIG_ARCH_CHIP} MATCHES "kinetis") + if(${CONFIG_NET_CAN} MATCHES "y") + set(UAVCAN_DRIVER "socketcan") + set(UAVCAN_TIMER 1) + elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis") set(UAVCAN_DRIVER "kinetis") set(UAVCAN_TIMER 1) elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") @@ -75,7 +78,7 @@ add_definitions( -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 - -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 + -DUAVCAN_MEM_POOL_BLOCK_SIZE=64 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=${UAVCAN_PLATFORM} ) diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index e808c5d01e90..864fb799c9f5 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -51,6 +51,7 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node) { _uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY); + param_get(param_find("UAVCAN_ESC_RL"), &_uavcan_rate_limit_enable); } int @@ -75,7 +76,7 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA */ const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + if (_uavcan_rate_limit_enable == 1 && (timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { return; } diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 4ff9c02ed8a8..41626e3d1b1a 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -105,6 +105,8 @@ class UavcanEscController uint8_t _rotor_count{0}; + int32_t _uavcan_rate_limit_enable{0}; + /* * libuavcan related things */ diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 023a16cf1d05..636cc18ed0cf 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -211,7 +211,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) channel->node_id = node_id; DEVICE_LOG("node %d instance %d ok", channel->node_id, channel->instance); - if (channel->orb_advert == nullptr) { + if (!orb_advert_valid(channel->orb_advert)) { DEVICE_LOG("uORB advertise failed. Out of instances?"); *channel = uavcan_bridge::Channel(); _out_of_channels = true; @@ -223,7 +223,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) assert(channel != nullptr); - (void)orb_publish(_orb_topic, channel->orb_advert, report); + (void)orb_publish(_orb_topic, &channel->orb_advert, report); } uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id) diff --git a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp index 44e0d1ad4fb3..f03f755a1bf8 100644 --- a/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp @@ -360,18 +360,13 @@ class CanInitHelper * This function can either initialize the driver at a fixed bit rate, or it can perform * automatic bit rate detection. For theory please refer to the CiA application note #801. * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. * If auto detection was used, the function will update the argument * with established bit rate. In case of an error the value will be undefined. * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) { if (inout_bitrate > 0) { return driver.init(inout_bitrate, CanIface::NormalMode); @@ -389,7 +384,7 @@ class CanInitHelper const int res = driver.init(inout_bitrate, CanIface::SilentMode); - delay_callable(); + usleep(1000000); if (res >= 0) { for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt index e3c5344d7989..0b7b785b8652 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt @@ -6,6 +6,7 @@ add_compile_options(-Wno-unused-variable) add_library(uavcan_socketcan_driver STATIC src/socketcan.cpp src/thread.cpp + src/clock.cpp ) add_dependencies(uavcan_socketcan_driver uavcan) diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp index 562fe054ec06..42ac63f54c05 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp @@ -46,6 +46,16 @@ namespace uavcan_socketcan { +namespace clock +{ +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +void adjustUtc(uavcan::UtcDuration adjustment); +//FIXME +} /** * Different adjustment modes can be used for time synchronization */ diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp index 75b904548901..12fc861400c7 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp @@ -71,7 +71,7 @@ class CanIface : public uavcan::ICanIface SystemClock clock; public: - uavcan::uint32_t socketInit(const char *can_iface_name); + uavcan::uint32_t socketInit(uint32_t index); uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, @@ -109,13 +109,13 @@ class CanDriver CanDriver() : update_event_(*this) {} - uavcan::int32_t initIface(uint32_t index, const char *name) + uavcan::int32_t initIface(uint32_t index) { if (index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) { return -1; } - return if_[index].socketInit(name); + return if_[index].socketInit(index); } /** @@ -180,10 +180,10 @@ class CanInitHelper */ int init(uavcan::uint32_t bitrate) { - driver.initIface(0, "can0"); -#if UAVCAN_SOCKETCAN_NUM_IFACES > 1 - driver.initIface(1, "can1"); -#endif + for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { + driver.initIface(i); + } + return driver.init(bitrate); } @@ -191,23 +191,20 @@ class CanInitHelper * This function can either initialize the driver at a fixed bit rate, or it can perform * automatic bit rate detection. For theory please refer to the CiA application note #801. * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * @param bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. * If auto detection was used, the function will update the argument * with established bit rate. In case of an error the value will be undefined. * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = 1000000) - { - if (inout_bitrate > 0) { - return driver.init(inout_bitrate); + int init(uavcan::uint32_t &bitrate = 1000000) + { + if (bitrate > 0) { + return driver.init(bitrate); } + + return -1; } /** diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp new file mode 100644 index 000000000000..6b030da82570 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * Kinetis Port Author David Sidrane + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +namespace uavcan_socketcan +{ +namespace clock +{ +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +void adjustUtc(uavcan::UtcDuration adjustment) +{ + //printf("Adjust UTC\n"); + //clock::adjustUtc(adjustment); +} + +} +} diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp index 422722b6ca5a..b3a8ce183625 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp @@ -69,7 +69,7 @@ struct BitTimingSettings { } // namespace -uavcan::uint32_t CanIface::socketInit(const char *can_iface_name) +uavcan::uint32_t CanIface::socketInit(uint32_t index) { struct sockaddr_can addr; @@ -86,7 +86,7 @@ uavcan::uint32_t CanIface::socketInit(const char *can_iface_name) return -1; } - strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1); + snprintf(ifr.ifr_name, IFNAMSIZ, "can%li", index); ifr.ifr_name[IFNAMSIZ - 1] = '\0'; ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); @@ -194,7 +194,7 @@ uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicT _send_tv->tv_usec = tx_deadline.toUSec() % 1000000ULL; _send_tv->tv_sec = (tx_deadline.toUSec() - _send_tv->tv_usec) / 1000000ULL; - res = sendmsg(_fd, &_send_msg, 0); + res = sendmsg(_fd, &_send_msg, MSG_DONTWAIT); if (res > 0) { return 1; @@ -271,12 +271,10 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) int CanDriver::init(uavcan::uint32_t bitrate) { - pfds[0].fd = if_[0].getFD(); - pfds[0].events = POLLIN | POLLOUT; -#if UAVCAN_SOCKETCAN_NUM_IFACES > 1 - pfds[1].fd = if_[1].getFD(); - pfds[1].events = POLLIN | POLLOUT; -#endif + for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { + pfds[i].fd = if_[i].getFD(); + pfds[i].events = POLLIN | POLLOUT; + } /* * TODO add filter configuration ioctl @@ -308,8 +306,7 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks, } inout_masks.read = 0; - //FIXME NuttX SocketCAN implement POLLOUT - inout_masks.write = 0x3; + inout_masks.write = 0; if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) { for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { diff --git a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index bc25be2a9ac1..67abcf53c58e 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -320,18 +320,14 @@ class CanInitHelper * This function can either initialize the driver at a fixed bit rate, or it can perform * automatic bit rate detection. For theory please refer to the CiA application note #801. * - * @param delay_callable A callable entity that suspends execution for strictly more than one second. - * The callable entity will be invoked without arguments. - * @ref getRecommendedListeningDelay(). - * - * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. * If auto detection was used, the function will update the argument * with established bit rate. In case of an error the value will be undefined. * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) { if (inout_bitrate > 0) { return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_); @@ -349,7 +345,7 @@ class CanInitHelper const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_); - delay_callable(); + usleep(1000000); if (res >= 0) { for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp index 22f8ff739ac9..9720b24967a3 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp @@ -341,8 +341,7 @@ class CanInitHelper * * @return Negative value on error; non-negative on success. Refer to constants Err*. */ - template - int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) + int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect) { if (inout_bitrate > 0) { return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_); @@ -360,7 +359,7 @@ class CanInitHelper const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_); - delay_callable(); + usleep(1000000); if (res >= 0) { for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 24b677da6f56..b0d3a0c77ca8 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -74,6 +74,8 @@ */ UavcanNode *UavcanNode::_instance; +static UavcanNode::CanInitHelper *can = nullptr; + UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), ModuleParams(nullptr), @@ -404,28 +406,15 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } - /* - * CAN driver init - * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver - * shipped with libuavcan does not support deinitialization. - */ - static CanInitHelper *can = nullptr; - if (can == nullptr) { can = new CanInitHelper(board_get_can_interfaces()); - if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown + if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown PX4_ERR("Out of memory"); return -1; } - const int can_init_res = can->init(bitrate); - - if (can_init_res < 0) { - PX4_ERR("CAN driver init failed %i", can_init_res); - return can_init_res; - } } /* @@ -438,15 +427,6 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } - const int node_init_res = _instance->init(node_id, can->driver.updateEvent()); - - if (node_init_res < 0) { - delete _instance; - _instance = nullptr; - PX4_ERR("Node init failed %i", node_init_res); - return node_init_res; - } - _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); _instance->_mixing_interface_esc.ScheduleNow(); _instance->_mixing_interface_servo.ScheduleNow(); @@ -517,10 +497,15 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return ret; } - ret = _safety_state_controller.init(); + int32_t safety_state_pub_enable = 0; + (void)param_get(param_find("UAVCAN_PUB_SS"), &safety_state_pub_enable); - if (ret < 0) { - return ret; + if (safety_state_pub_enable == 1) { + ret = _safety_state_controller.init(); + + if (ret < 0) { + return ret; + } } ret = _log_message_controller.init(); @@ -529,10 +514,15 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return ret; } - ret = _rgbled_controller.init(); + int32_t rgbled_pub_enable = 0; + (void)param_get(param_find("UAVCAN_PUB_RGB"), &rgbled_pub_enable); - if (ret < 0) { - return ret; + if (rgbled_pub_enable == 1) { + ret = _rgbled_controller.init(); + + if (ret < 0) { + return ret; + } } /* Start node info retriever to fetch node info from new nodes */ @@ -571,18 +561,19 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode)); _param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart)); - int32_t uavcan_enable = 1; (void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable); + int32_t uavcan_dynamic_node_server_enable = 1; + (void)param_get(param_find("UAVCAN_DNS"), &uavcan_dynamic_node_server_enable); - if (uavcan_enable > 1) { + if (uavcan_enable > 1 && uavcan_dynamic_node_server_enable == 1) { _servers = new UavcanServers(_node, _node_info_retriever); if (_servers) { int rv = _servers->init(); if (rv < 0) { - PX4_ERR("UavcanServers init: %d", ret); + PX4_ERR("UavcanServers init: %d", rv); } } } @@ -634,6 +625,36 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &) void UavcanNode::Run() { + if (!_node_init) { + // Node ID + int32_t node_id = 1; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); + + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + PX4_ERR("Invalid Node ID %" PRId32, node_id); + ::exit(1); + } + + // CAN bitrate + int32_t bitrate = 1000000; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); + + /* + * CAN driver init + * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver + * shipped with libuavcan does not support deinitialization. + */ + const int can_init_res = can->init(bitrate); + + if (can_init_res < 0) { + PX4_ERR("CAN driver init failed %i", can_init_res); + } + + _instance->init(node_id, can->driver.updateEvent()); + + _node_init = true; + } + pthread_mutex_lock(&_node_mutex); if (_output_count == 0) { @@ -1100,11 +1121,22 @@ UavcanNode::cb_getset(const uavcan::ServiceCallResult(); } - + PX4_DEBUG("Got node : %d, param index %d", response.node_id, response.param_index); _param_response_pub.publish(response); } else { - PX4_ERR("GetSet error"); + PX4_ERR("GetSet error at node : %d, param index %d, need resend...", result.getCallID().server_node_id.get(), _param_index); + + uavcan::protocol::param::GetSet::Request req; + + req.index = _param_index; + + int call_res = _param_getset_client.call(result.getCallID().server_node_id.get(), req); + if(call_res < 0){ + PX4_ERR("resend failed"); + } + + _param_index--; } _param_in_progress = false; diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 2bb8a434c0d4..ee0e1f8c04db 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -220,6 +220,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams uavcan_node::Allocator _pool_allocator; + bool _node_init{false}; uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; @@ -264,7 +265,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)}; - uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)}; + uORB::SubscriptionInterval _param_request_sub{ORB_ID(uavcan_parameter_request), 100_us}; uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index ce6b34480c93..ff508554a665 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -209,6 +209,30 @@ PARAM_DEFINE_INT32(UAVCAN_PUB_RTCM, 0); */ PARAM_DEFINE_INT32(UAVCAN_PUB_MBD, 0); +/** + * publish safety state + * + * Enable UAVCAN safety state publish to control CAN SafetyState led + * ardupilot::indication::SafetyState + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_SS, 0); + +/** + * publish rgb led + * + * Enable UAVCAN rgb led control + * uavcan::equipment::indication::LightsCommand + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_RGB, 0); + /** * subscription airspeed * @@ -366,3 +390,26 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_RNG, 0); * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_SUB_BTN, 0); + +/** + * use rate limit of ESC output + * + * Enable UAVCAN ESC rate limit for output update + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ESC_RL, 0); + +/** + * use UAVCAN dynamic node server + * + * Enable UAVCAN dynamic node server for automatic node enumeration + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_DNS, 0); + diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 9d64c9105e01..f60e76aea48b 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -365,7 +365,7 @@ void UavcanNode::Run() if (!_initialized) { - const int can_init_res = _can->init(_bitrate); + const int can_init_res = _can->init((uint32_t)_bitrate); if (can_init_res < 0) { PX4_ERR("CAN driver init failed %i", can_init_res); diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 5972a2b64d69..e8c2a7d52678 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -69,7 +69,7 @@ int ex_hwtest_main(int argc, char *argv[]) arm.ready_to_arm = true; arm.armed = true; orb_advert_t arm_pub_ptr = orb_advertise(ORB_ID(actuator_armed), &arm); - orb_publish(ORB_ID(actuator_armed), arm_pub_ptr, &arm); + orb_publish(ORB_ID(actuator_armed), &arm_pub_ptr, &arm); /* read back values to validate */ int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); @@ -115,7 +115,7 @@ int ex_hwtest_main(int argc, char *argv[]) } actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_0), actuator_pub_ptr, &actuators); + orb_publish(ORB_ID(actuator_controls_0), &actuator_pub_ptr, &actuators); usleep(10000); } diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp b/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp index 97047f074842..568526c74310 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp @@ -93,19 +93,19 @@ int px4_mavlink_debug_main(int argc, char *argv[]) /* send one named value */ dbg_key.value = value_counter; dbg_key.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_key_value), pub_dbg_key, &dbg_key); + orb_publish(ORB_ID(debug_key_value), &pub_dbg_key, &dbg_key); /* send one indexed value */ dbg_ind.value = 0.5f * value_counter; dbg_ind.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_value), pub_dbg_ind, &dbg_ind); + orb_publish(ORB_ID(debug_value), &pub_dbg_ind, &dbg_ind); /* send one vector */ dbg_vect.x = 1.0f * value_counter; dbg_vect.y = 2.0f * value_counter; dbg_vect.z = 3.0f * value_counter; dbg_vect.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_vect), pub_dbg_vect, &dbg_vect); + orb_publish(ORB_ID(debug_vect), &pub_dbg_vect, &dbg_vect); /* send one array */ for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) { @@ -113,7 +113,7 @@ int px4_mavlink_debug_main(int argc, char *argv[]) } dbg_array.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_array), pub_dbg_array, &dbg_array); + orb_publish(ORB_ID(debug_array), &pub_dbg_array, &dbg_array); warnx("sent one more value.."); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 25e89efcf1f3..72e8abcf51e5 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[]) PX4_INFO("Hello Sky!"); /* subscribe to vehicle_acceleration topic */ - int sensor_sub_fd = orb_subscribe(ORB_ID(vehicle_acceleration)); + orb_sub_t sensor_sub_fd = orb_subscribe(ORB_ID(vehicle_acceleration)); /* limit the update rate to 5 Hz */ orb_set_interval(sensor_sub_fd, 200); @@ -115,7 +115,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.q[1] = accel.xyz[1]; att.q[2] = accel.xyz[2]; - orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); + orb_publish(ORB_ID(vehicle_attitude), &att_pub, &att); } /* there could be more file descriptors here, in the form like: diff --git a/src/include/visibility.h b/src/include/visibility.h index bc38ffb8a1ac..db856dd65349 100644 --- a/src/include/visibility.h +++ b/src/include/visibility.h @@ -123,7 +123,20 @@ /* We should include cstdlib or stdlib.h but this doesn't * compile because many C++ files include stdlib.h and would * need to get changed. */ +#ifndef CONFIG_LIB_SYSCALL +/* The syscall interface puts these into the syscall LUT. When PX4 and NuttX + * are compiled as one, it is impossible to reliably limit visibility to these + * symbols only for the kernel. + * + * This produces false alarms via: + * /sys/syscall_lookup.h:344:18: error: attempt to use poisoned "getenv" + * 344 | SYSCALL_LOOKUP(getenv, 1) + * + * This alarm is false, as the symbol is not really _used_ from anywhere, the + * kernel simply populates it (and the empty handler) into the syscall table. + */ #pragma GCC poison getenv setenv putenv +#endif #endif // defined(__PX4_NUTTX) #endif // PX4_DISABLE_GCC_POISON diff --git a/src/lib/cdev/CDev.cpp b/src/lib/cdev/CDev.cpp index b5bcce2dd34b..0b90dc845c45 100644 --- a/src/lib/cdev/CDev.cpp +++ b/src/lib/cdev/CDev.cpp @@ -67,10 +67,6 @@ CDev::~CDev() unregister_driver(_devname); } - if (_pollset) { - delete[](_pollset); - } - px4_sem_destroy(&_lock); } @@ -191,190 +187,4 @@ CDev::close(file_t *filep) return ret; } -int -CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) -{ - PX4_DEBUG("CDev::Poll %s", setup ? "setup" : "teardown"); - int ret = PX4_OK; - - if (setup) { - /* - * Save the file pointer in the pollfd for the subclass' - * benefit. - */ - fds->priv = (void *)filep; - PX4_DEBUG("CDev::poll: fds->priv = %p", filep); - - /* - * Lock against poll_notify() and possibly other callers (protect _pollset). - */ - ATOMIC_ENTER; - - /* - * Try to store the fds for later use and handle array resizing. - */ - while ((ret = store_poll_waiter(fds)) == -ENFILE) { - - // No free slot found. Resize the pollset. This is expensive, but it's only needed initially. - - if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t - ret = -ENOMEM; - break; - } - - const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1; - px4_pollfd_struct_t **prev_pollset = _pollset; - -#ifdef __PX4_NUTTX - // malloc uses a semaphore, we need to call it enabled IRQ's - px4_leave_critical_section(flags); -#endif - px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count]; - -#ifdef __PX4_NUTTX - flags = px4_enter_critical_section(); -#endif - - if (prev_pollset == _pollset) { - // no one else updated the _pollset meanwhile, so we're good to go - if (!new_pollset) { - ret = -ENOMEM; - break; - } - - if (_max_pollwaiters > 0) { - memset(new_pollset + _max_pollwaiters, 0, sizeof(px4_pollfd_struct_t *) * (new_count - _max_pollwaiters)); - memcpy(new_pollset, _pollset, sizeof(px4_pollfd_struct_t *) * _max_pollwaiters); - } - - _pollset = new_pollset; - _pollset[_max_pollwaiters] = fds; - _max_pollwaiters = new_count; - - // free the previous _pollset (we need to unlock here which is fine because we don't access _pollset anymore) -#ifdef __PX4_NUTTX - px4_leave_critical_section(flags); -#endif - - if (prev_pollset) { - delete[](prev_pollset); - } - -#ifdef __PX4_NUTTX - flags = px4_enter_critical_section(); -#endif - - // Success - ret = PX4_OK; - break; - } - -#ifdef __PX4_NUTTX - px4_leave_critical_section(flags); -#endif - // We have to retry - delete[] new_pollset; -#ifdef __PX4_NUTTX - flags = px4_enter_critical_section(); -#endif - } - - if (ret == PX4_OK) { - - /* - * Check to see whether we should send a poll notification - * immediately. - */ - fds->revents |= fds->events & poll_state(filep); - - /* yes? post the notification */ - if (fds->revents != 0) { - px4_sem_post(fds->sem); - } - - } - - ATOMIC_LEAVE; - - } else { - ATOMIC_ENTER; - /* - * Handle a teardown request. - */ - ret = remove_poll_waiter(fds); - ATOMIC_LEAVE; - } - - return ret; -} - -void -CDev::poll_notify(px4_pollevent_t events) -{ - PX4_DEBUG("CDev::poll_notify events = %0x", events); - - /* lock against poll() as well as other wakeups */ - ATOMIC_ENTER; - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - if (nullptr != _pollset[i]) { - poll_notify_one(_pollset[i], events); - } - } - - ATOMIC_LEAVE; -} - -void -CDev::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) -{ - PX4_DEBUG("CDev::poll_notify_one"); - - /* update the reported event set */ - fds->revents |= fds->events & events; - - PX4_DEBUG(" Events fds=%p %0x %0x %0x", fds, fds->revents, fds->events, events); - - if (fds->revents != 0) { - px4_sem_post(fds->sem); - } -} - -int -CDev::store_poll_waiter(px4_pollfd_struct_t *fds) -{ - // Look for a free slot. - PX4_DEBUG("CDev::store_poll_waiter"); - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - if (nullptr == _pollset[i]) { - - /* save the pollfd */ - _pollset[i] = fds; - - return PX4_OK; - } - } - - return -ENFILE; -} - -int -CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) -{ - PX4_DEBUG("CDev::remove_poll_waiter"); - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - if (fds == _pollset[i]) { - - _pollset[i] = nullptr; - return PX4_OK; - - } - } - - PX4_DEBUG("poll: bad fd state"); - return -EINVAL; -} - } // namespace cdev diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index 071ae618fe97..41818f5fb630 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -148,19 +148,6 @@ class __EXPORT CDev */ virtual int ioctl(file_t *filep, int cmd, unsigned long arg) { return -ENOTTY; }; - /** - * Perform a poll setup/teardown operation. - * - * This is handled internally and should not normally be overridden. - * - * @param filep Pointer to the internal file structure. - * @param fds Poll descriptor being waited on. - * @param setup True if this is establishing a request, false if - * it is being torn down. - * @return OK on success, or -errno otherwise. - */ - int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup); - /** * Get the device name. * @@ -175,38 +162,6 @@ class __EXPORT CDev */ static const px4_file_operations_t fops; - /** - * Check the current state of the device for poll events from the - * perspective of the file. - * - * This function is called by the default poll() implementation when - * a poll is set up to determine whether the poll should return immediately. - * - * The default implementation returns no events. - * - * @param filep The file that's interested. - * @return The current set of poll events. - */ - virtual px4_pollevent_t poll_state(file_t *filep) { return 0; } - - /** - * Report new poll events. - * - * This function should be called anytime the state of the device changes - * in a fashion that might be interesting to a poll waiter. - * - * @param events The new event(s) being announced. - */ - void poll_notify(px4_pollevent_t events); - - /** - * Internal implementation of poll_notify. - * - * @param fds A poll waiter to notify. - * @param events The event(s) to send to the waiter. - */ - virtual void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events); - /** * Notification of the first open. * @@ -273,29 +228,10 @@ class __EXPORT CDev private: const char *_devname{nullptr}; /**< device node name */ - px4_pollfd_struct_t **_pollset{nullptr}; - bool _registered{false}; /**< true if device name was registered */ - uint8_t _max_pollwaiters{0}; /**< size of the _pollset array */ uint16_t _open_count{0}; /**< number of successful opens */ - /** - * Store a pollwaiter in a slot where we can find it later. - * - * Expands the pollset as required. Must be called with the driver locked. - * - * @return OK, or -errno on error. - */ - inline int store_poll_waiter(px4_pollfd_struct_t *fds); - - /** - * Remove a poll waiter. - * - * @return OK, or -errno on error. - */ - inline int remove_poll_waiter(px4_pollfd_struct_t *fds); - }; } // namespace cdev diff --git a/src/lib/cdev/nuttx/cdev_platform.cpp b/src/lib/cdev/nuttx/cdev_platform.cpp index 39de49a55c05..3d0c01e5ae0d 100644 --- a/src/lib/cdev/nuttx/cdev_platform.cpp +++ b/src/lib/cdev/nuttx/cdev_platform.cpp @@ -60,7 +60,6 @@ static ssize_t cdev_read(file_t *filp, char *buffer, size_t buflen); static ssize_t cdev_write(file_t *filp, const char *buffer, size_t buflen); static off_t cdev_seek(file_t *filp, off_t offset, int whence); static int cdev_ioctl(file_t *filp, int cmd, unsigned long arg); -static int cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup); /** * Character device indirection table. @@ -78,7 +77,7 @@ read : cdev_read, write : cdev_write, seek : cdev_seek, ioctl : cdev_ioctl, -poll : cdev_poll, +poll : nullptr, #ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS unlink : nullptr #endif @@ -156,16 +155,4 @@ cdev_ioctl(file_t *filp, int cmd, unsigned long arg) return cdev->ioctl(filp, cmd, arg); } -static int -cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup) -{ - if ((filp->f_inode->i_flags & FSNODEFLAG_DELETED) != 0) { - return -ENODEV; - } - - cdev::CDev *cdev = (cdev::CDev *)(filp->f_inode->i_private); - - return cdev->poll(filp, fds, setup); -} - } // namespace cdev diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp index 0a977a1bba16..4f17d473e7d7 100644 --- a/src/lib/cdev/posix/cdev_platform.cpp +++ b/src/lib/cdev/posix/cdev_platform.cpp @@ -76,8 +76,7 @@ class VFile : public cdev::CDev ssize_t write(cdev::file_t *handlep, const char *buffer, size_t buflen) override { - // ignore what was written, but let pollers know something was written - poll_notify(POLLIN); + // ignore what was written return buflen; } }; @@ -329,119 +328,6 @@ extern "C" { return ret; } - int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout) - { - if (nfds == 0) { - PX4_WARN("px4_poll with no fds"); - return -1; - } - - px4_sem_t sem; - int count = 0; - int ret = -1; - - const unsigned NAMELEN = 32; - char thread_name[NAMELEN] {}; - -#ifndef __PX4_QURT - int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); - - if (nret || thread_name[0] == 0) { - PX4_WARN("failed getting thread name"); - } - -#endif - - PX4_DEBUG("Called px4_poll timeout = %d", timeout); - - px4_sem_init(&sem, 0, 0); - - // sem use case is a signal - px4_sem_setprotocol(&sem, SEM_PRIO_NONE); - - // Go through all fds and check them for a pollable state - bool fd_pollable = false; - - for (unsigned int i = 0; i < nfds; ++i) { - fds[i].sem = &sem; - fds[i].revents = 0; - fds[i].priv = nullptr; - - cdev::CDev *dev = getFile(fds[i].fd); - - // If fd is valid - if (dev) { - PX4_DEBUG("%s: px4_poll: CDev->poll(setup) %d", thread_name, fds[i].fd); - ret = dev->poll(&filemap[fds[i].fd], &fds[i], true); - - if (ret < 0) { - PX4_WARN("%s: px4_poll() error: %s", thread_name, strerror(errno)); - break; - } - - if (ret >= 0) { - fd_pollable = true; - } - } - } - - // If any FD can be polled, lock the semaphore and - // check for new data - if (fd_pollable) { - if (timeout > 0) { - // Get the current time - struct timespec ts; - // Note, we can't actually use CLOCK_MONOTONIC on macOS - // but that's hidden and implemented in px4_clock_gettime. - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - - // Calculate an absolute time in the future - const unsigned billion = (1000 * 1000 * 1000); - uint64_t nsecs = ts.tv_nsec + ((uint64_t)timeout * 1000 * 1000); - ts.tv_sec += nsecs / billion; - nsecs -= (nsecs / billion) * billion; - ts.tv_nsec = nsecs; - - ret = px4_sem_timedwait(&sem, &ts); - - if (ret && errno != ETIMEDOUT) { - PX4_WARN("%s: px4_poll() sem error: %s", thread_name, strerror(errno)); - } - - } else if (timeout < 0) { - px4_sem_wait(&sem); - } - - // We have waited now (or not, depending on timeout), - // go through all fds and count how many have data - for (unsigned int i = 0; i < nfds; ++i) { - - cdev::CDev *dev = getFile(fds[i].fd); - - // If fd is valid - if (dev) { - PX4_DEBUG("%s: px4_poll: CDev->poll(teardown) %d", thread_name, fds[i].fd); - ret = dev->poll(&filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) { - PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name); - break; - } - - if (fds[i].revents) { - count += 1; - } - } - } - } - - px4_sem_destroy(&sem); - - // Return the positive count if present, - // return the negative error number if failed - return (count) ? count : ret; - } - int px4_access(const char *pathname, int mode) { if (mode != F_OK) { diff --git a/src/lib/cdev/test/cdevtest_example.cpp b/src/lib/cdev/test/cdevtest_example.cpp index 48bed08d0b82..9944ff746b4e 100644 --- a/src/lib/cdev/test/cdevtest_example.cpp +++ b/src/lib/cdev/test/cdevtest_example.cpp @@ -162,9 +162,6 @@ ssize_t CDevNode::write(cdev::file_t *handlep, const char *buffer, size_t buflen _write_offset++; } - // ignore what was written, but let pollers know something was written - poll_notify(POLLIN); - return buflen; } @@ -191,59 +188,6 @@ CDevExample::~CDevExample() } } -int CDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll) -{ - int pollret, readret; - int loop_count = 0; - char readbuf[10]; - px4_pollfd_struct_t fds[1]; - - fds[0].fd = fd; - fds[0].events = POLLIN; - fds[0].revents = 0; - - bool mustblock = (timeout < 0); - - // Test indefinte blocking poll - while ((!appState.exitRequested()) && (loop_count < iterations)) { - pollret = px4_poll(fds, 1, timeout); - - if (pollret < 0) { - PX4_ERR("Reader: px4_poll failed %d FAIL", pollret); - goto fail; - } - - PX4_INFO("Reader: px4_poll returned %d", pollret); - - if (pollret) { - readret = px4_read(fd, readbuf, 10); - - if (readret != 1) { - if (mustblock) { - PX4_ERR("Reader: read failed %d FAIL", readret); - goto fail; - - } else { - PX4_INFO("Reader: read failed %d FAIL", readret); - } - - } else { - readbuf[readret] = '\0'; - PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); - } - } - - if (delayms_after_poll) { - px4_usleep(delayms_after_poll * 1000); - } - - loop_count++; - } - - return 0; -fail: - return 1; -} int CDevExample::main() { appState.setRunning(true); @@ -277,52 +221,7 @@ int CDevExample::main() int ret = 0; - PX4_INFO("TEST: BLOCKING POLL ---------------"); - - if (do_poll(fd, -1, 3, 0)) { - ret = 1; - goto fail2; - } - - PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 3, 0)) { - ret = 1; - goto fail2; - goto fail2; - } - - PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 3, 0)) { - ret = 1; - goto fail2; - goto fail2; - } - - PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 3, 0)) { - ret = 1; - goto fail2; - } - - PX4_INFO("TEST: 100ms TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 30, 100)) { - ret = 1; - goto fail2; - } - - PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); - - if (do_poll(fd, 1000, 3, 0)) { - ret = 1; - goto fail2; - } - PX4_INFO("TEST: waiting for writer to stop"); -fail2: g_exit = true; px4_close(fd); PX4_INFO("TEST: waiting for writer to stop"); diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa00..ef8cc169438f 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -170,8 +170,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); matrix::Vector2f modified_setpoint1 = original_setpoint1; matrix::Vector2f modified_setpoint2 = original_setpoint2; cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); @@ -228,8 +228,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(distance_sensor), distance_sensor_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(distance_sensor), &distance_sensor_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); //WHEN: We run the setpoint modification matrix::Vector2f modified_setpoint1 = original_setpoint1; @@ -302,8 +302,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { @@ -312,7 +312,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) mocked_time = mocked_time + 100000; //advance time by 0.1 seconds message_lost_data.timestamp = mocked_time; - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_lost_data); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message_lost_data); //at iteration 8 change the CP_GO_NO_DATA to True if (i == 8) { @@ -380,8 +380,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { @@ -437,7 +437,7 @@ TEST_F(CollisionPreventionTest, noBias) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); matrix::Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -492,7 +492,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) matrix::Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; matrix::Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); //THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV @@ -567,7 +567,7 @@ TEST_F(CollisionPreventionTest, goNoData) //THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed message.timestamp = hrt_absolute_time(); orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); @@ -606,7 +606,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) // AND: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -1112,8 +1112,8 @@ TEST_F(CollisionPreventionTest, overlappingSensors) //WHEN: we publish the long range sensor message orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &long_range_msg); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &long_range_msg); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); matrix::Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); @@ -1123,10 +1123,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // CASE 2 // WHEN: we publish the short range message followed by a long range message short_range_msg.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &short_range_msg); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &long_range_msg); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); // THEN: the internal map data should contain the short range measurement @@ -1135,10 +1135,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // CASE 3 // WHEN: we publish the short range message with values out of range followed by a long range message short_range_msg_no_obstacle.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &short_range_msg_no_obstacle); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &long_range_msg); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); // THEN: the internal map data should contain the short range measurement diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp index 176928923af0..ad817e983ad9 100644 --- a/src/lib/drivers/smbus_sbs/SBS.hpp +++ b/src/lib/drivers/smbus_sbs/SBS.hpp @@ -321,6 +321,6 @@ void SMBUS_SBS_BaseClass::RunImpl() // Only publish if no errors. if (!ret) { - orb_publish(ORB_ID(battery_status), _batt_topic, &new_report); + orb_publish(ORB_ID(battery_status), &_batt_topic, &new_report); } } diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 08fd1202f730..3842c0f04fca 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -97,7 +97,7 @@ class SecondOrderReferenceModel // Zero natural frequency means our time step is irrelevant cutoff_freq_ = FLT_EPSILON; - max_time_step_ = INFINITY; + max_time_step_ = (float)INFINITY; // Fail return false; @@ -198,7 +198,7 @@ class SecondOrderReferenceModel T last_rate_sample_{}; // [units/s] // Maximum time step [s] - float max_time_step_{INFINITY}; + float max_time_step_{(float)INFINITY}; // The selected time discretization method used for state integration DiscretizationMethod discretization_method_{kBilinear}; diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp index b0e04923e915..c3a16f02c386 100644 --- a/src/lib/parameters/ParameterTest.cpp +++ b/src/lib/parameters/ParameterTest.cpp @@ -92,7 +92,7 @@ TEST_F(ParameterTest, testUorbSendReceive) // WHEN we send the message orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - ASSERT_TRUE(obstacle_distance_pub != nullptr); + ASSERT_TRUE(orb_advert_valid(obstacle_distance_pub)); // THEN: the subscriber should receive the message sub_obstacle_distance.update(); diff --git a/src/lib/parameters/flashparams/flashfs.c b/src/lib/parameters/flashparams/flashfs.c index a692b62048dc..bbbab3a0abb8 100644 --- a/src/lib/parameters/flashparams/flashfs.c +++ b/src/lib/parameters/flashparams/flashfs.c @@ -44,7 +44,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include #include diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 0822842e4566..3c9519a4f58b 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -47,7 +47,11 @@ #include #include "tinybson/tinybson.h" +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include @@ -113,7 +117,7 @@ UT_array *param_custom_default_values{nullptr}; const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr}; /** parameter update topic handle */ -static orb_advert_t param_topic = nullptr; +static orb_advert_t param_topic = ORB_ADVERT_INVALID; static unsigned int param_instance = 0; // the following implements an RW-lock using 2 semaphores (used as mutexes). It gives @@ -260,11 +264,11 @@ param_notify_changes() pup.custom_default = params_custom_default.count(); pup.timestamp = hrt_absolute_time(); - if (param_topic == nullptr) { + if (!orb_advert_valid(param_topic)) { param_topic = orb_advertise(ORB_ID(parameter_update), &pup); } else { - orb_publish(ORB_ID(parameter_update), param_topic, &pup); + orb_publish(ORB_ID(parameter_update), ¶m_topic, &pup); } } diff --git a/src/lib/systemlib/mavlink_log.cpp b/src/lib/systemlib/mavlink_log.cpp index d31136f21530..4ba498d40745 100644 --- a/src/lib/systemlib/mavlink_log.cpp +++ b/src/lib/systemlib/mavlink_log.cpp @@ -56,7 +56,7 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con return; } - if (mavlink_log_pub == nullptr) { + if (mavlink_log_pub == nullptr || !orb_advert_valid(*mavlink_log_pub)) { return; } @@ -69,8 +69,8 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap); va_end(ap); - if (*mavlink_log_pub != nullptr) { - orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); + if (orb_advert_valid(*mavlink_log_pub)) { + orb_publish(ORB_ID(mavlink_log), mavlink_log_pub, &log_msg); } else { *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH); diff --git a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp index bfcff4a3aead..a9975f225445 100644 --- a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp +++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -46,7 +46,7 @@ using namespace time_literals; static orb_advert_t *mavlink_log_pub; -static int command_ack_sub = -1; +static orb_sub_t command_ack_sub = ORB_SUB_INVALID; static hrt_abstime auth_timeout; static hrt_abstime auth_req_time; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1e012bafe06c..46afe4b0b991 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -98,7 +98,7 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review"); #if defined(BOARD_HAS_POWER_CONTROL) -static orb_advert_t tune_control_pub = nullptr; +static orb_advert_t tune_control_pub = ORB_ADVERT_INVALID; static void play_power_button_down_tune() { @@ -111,10 +111,10 @@ static void stop_tune() tune_control_s tune_control{}; tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + orb_publish(ORB_ID(tune_control), &tune_control_pub, &tune_control); } -static orb_advert_t power_button_state_pub = nullptr; +static orb_advert_t power_button_state_pub = ORB_ADVERT_INVALID; static int power_button_state_notification_cb(board_power_button_state_notification_e request) { // Note: this can be called from IRQ handlers, so we publish a message that will be handled @@ -147,8 +147,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat return ret; } - if (power_button_state_pub != nullptr) { - orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state); + if (!orb_advert_valid(power_button_state_pub)) { + orb_publish(ORB_ID(power_button_state), &power_button_state_pub, &button_state); } else { PX4_ERR("power_button_state_pub not properly initialized"); @@ -1704,9 +1704,14 @@ void Commander::run() perf_begin(_preflight_check_perf); _health_and_arming_checks.update(); - _vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state); - perf_end(_preflight_check_perf); + bool pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state); + if (_vehicle_status.pre_flight_checks_pass != pre_flight_checks_pass) { + _vehicle_status.pre_flight_checks_pass = pre_flight_checks_pass; + _status_changed = true; + } + + perf_end(_preflight_check_perf); checkAndInformReadyForTakeoff(); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 0ea42d18325b..11d9fee5f88a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -33,6 +33,8 @@ #pragma once +#include + /* Helper classes */ #include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "failure_detector/FailureDetector.hpp" diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 4790d3d2f73a..2757c11a0d45 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -128,7 +128,7 @@ static hrt_abstime blink_msg_end = 0; static int fd_leds{-1}; static led_control_s led_control {}; -static orb_advert_t led_control_pub = nullptr; +static orb_advert_t led_control_pub = ORB_ADVERT_INVALID; // Static array that defines the duration of each tune, 0 if it's a repeating tune (therefore no fixed duration) static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {}; @@ -140,7 +140,7 @@ static hrt_abstime tune_end = 0; static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; static tune_control_s tune_control {}; -static orb_advert_t tune_control_pub = nullptr; +static orb_advert_t tune_control_pub = ORB_ADVERT_INVALID; int buzzer_init() @@ -170,7 +170,7 @@ void set_tune_override(const int tune_id) tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + orb_publish(ORB_ID(tune_control), &tune_control_pub, &tune_control); } void set_tune(const int tune_id) @@ -205,7 +205,7 @@ void set_tune(const int tune_id) tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; tune_control.tune_override = false; tune_control.timestamp = current_time; - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + orb_publish(ORB_ID(tune_control), &tune_control_pub, &tune_control); tune_current = tune_id; @@ -391,7 +391,7 @@ void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint led_control.num_blinks = blinks; led_control.priority = prio; led_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(led_control), led_control_pub, &led_control); + orb_publish(ORB_ID(led_control), &led_control_pub, &led_control); } void rgbled_set_color_and_mode(uint8_t color, uint8_t mode) diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index e2e8e99b4cfc..46c915ef8f80 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -77,7 +77,8 @@ bool check_battery_disconnected(orb_advert_t *mavlink_log_pub) return false; } -static void set_motor_actuators(uORB::Publication &publisher, float value, bool release_control) +static void set_motor_actuators(uORB::Publication &publisher, + float value, bool release_control) { actuator_test_s actuator_test{}; actuator_test.timestamp = hrt_absolute_time(); @@ -96,7 +97,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); int return_code = PX4_OK; - uORB::Publication actuator_test_pub{ORB_ID(actuator_test)}; + uORB::Publication actuator_test_pub{ORB_ID(actuator_test)}; // since we publish multiple at once, make sure the output driver subscribes before we publish actuator_test_pub.advertise(); px4_usleep(10000); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9903990a5344..1996bf650de3 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -417,11 +417,11 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } - if (worker_data->mag_worker_data_pub == nullptr) { + if (!orb_advert_valid(worker_data->mag_worker_data_pub)) { worker_data->mag_worker_data_pub = orb_advertise(ORB_ID(mag_worker_data), &status); } else { - orb_publish(ORB_ID(mag_worker_data), worker_data->mag_worker_data_pub, &status); + orb_publish(ORB_ID(mag_worker_data), &worker_data->mag_worker_data_pub, &status); } calibration_counter_side++; @@ -484,7 +484,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma mag_worker_data_t worker_data{}; - worker_data.mag_worker_data_pub = nullptr; + worker_data.mag_worker_data_pub = ORB_ADVERT_INVALID; // keep and update the existing calibration when we are not doing a full 6-axis calibration worker_data.append_to_existing_calibration = cal_mask < ((1 << 6) - 1); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f5a7aaedbb8f..264215399850 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -139,7 +139,7 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::VectorgetActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); - actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : (float)NAN; if (stopped_motors & (1u << motors_idx)) { - actuator_motors.control[motors_idx] = NAN; + actuator_motors.control[motors_idx] = (float)NAN; } ++actuator_idx_matrix[selected_matrix]; @@ -668,7 +668,7 @@ ControlAllocator::publish_actuator_controls() } for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) { - actuator_motors.control[i] = NAN; + actuator_motors.control[i] = (float)NAN; } _actuator_motors_pub.publish(actuator_motors); @@ -680,13 +680,13 @@ ControlAllocator::publish_actuator_controls() for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); - actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : (float)NAN; ++actuator_idx_matrix[selected_matrix]; ++actuator_idx; } for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) { - actuator_servos.control[i] = NAN; + actuator_servos.control[i] = (float)NAN; } _actuator_servos_pub.publish(actuator_servos); diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index ffef587b2726..71fd8fd653d2 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -466,7 +466,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) break; } - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_info(&mavlink_log_pub, "Autotune returned to idle"); _state = state::idle; _param_fw_at_start.set(false); @@ -482,7 +482,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) if (now - _state_start_time > 20_s || (_param_fw_at_man_aux.get() && !_aux_switch_en) || _start_flight_mode != _nav_state) { - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing"); _state = state::fail; _state_start_time = now; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index 58c83298f91f..63e8452b4c75 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -51,11 +51,11 @@ InputMavlinkROI::InputMavlinkROI(Parameters ¶meters) : InputMavlinkROI::~InputMavlinkROI() { - if (_vehicle_roi_sub >= 0) { + if (orb_sub_valid(_vehicle_roi_sub)) { orb_unsubscribe(_vehicle_roi_sub); } - if (_position_setpoint_triplet_sub >= 0) { + if (orb_sub_valid(_position_setpoint_triplet_sub)) { orb_unsubscribe(_position_setpoint_triplet_sub); } } @@ -64,13 +64,13 @@ int InputMavlinkROI::initialize() { _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); - if (_vehicle_roi_sub < 0) { + if (!orb_sub_valid(_vehicle_roi_sub)) { return -errno; } _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - if (_position_setpoint_triplet_sub < 0) { + if (!orb_sub_valid(_position_setpoint_triplet_sub)) { return -errno; } @@ -171,14 +171,14 @@ InputMavlinkCmdMount::InputMavlinkCmdMount(Parameters ¶meters) : InputMavlinkCmdMount::~InputMavlinkCmdMount() { - if (_vehicle_command_sub >= 0) { + if (orb_sub_valid(_vehicle_command_sub)) { orb_unsubscribe(_vehicle_command_sub); } } int InputMavlinkCmdMount::initialize() { - if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + if (!orb_sub_valid(_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)))) { return -errno; } @@ -381,23 +381,23 @@ InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters ¶meters) : InputMavlinkGimbalV2::~InputMavlinkGimbalV2() { - if (_vehicle_roi_sub >= 0) { + if (orb_sub_valid(_vehicle_roi_sub)) { orb_unsubscribe(_vehicle_roi_sub); } - if (_position_setpoint_triplet_sub >= 0) { + if (orb_sub_valid(_position_setpoint_triplet_sub)) { orb_unsubscribe(_position_setpoint_triplet_sub); } - if (_gimbal_manager_set_attitude_sub >= 0) { + if (orb_sub_valid(_gimbal_manager_set_attitude_sub)) { orb_unsubscribe(_gimbal_manager_set_attitude_sub); } - if (_vehicle_command_sub >= 0) { + if (orb_sub_valid(_vehicle_command_sub)) { orb_unsubscribe(_vehicle_command_sub); } - if (_gimbal_manager_set_manual_control_sub >= 0) { + if (orb_sub_valid(_gimbal_manager_set_manual_control_sub)) { orb_unsubscribe(_gimbal_manager_set_manual_control_sub); } } @@ -412,27 +412,27 @@ int InputMavlinkGimbalV2::initialize() { _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); - if (_vehicle_roi_sub < 0) { + if (!orb_sub_valid(_vehicle_roi_sub)) { return -errno; } _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - if (_position_setpoint_triplet_sub < 0) { + if (!orb_sub_valid(_position_setpoint_triplet_sub)) { return -errno; } _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); - if (_gimbal_manager_set_attitude_sub < 0) { + if (!orb_sub_valid(_gimbal_manager_set_attitude_sub)) { return -errno; } - if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + if (!orb_sub_valid(_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)))) { return -errno; } - if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) { + if (!orb_sub_valid(_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control)))) { return -errno; } diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index 0bc0a7f1afb2..e65a13f9c027 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -68,8 +68,8 @@ class InputMavlinkROI : public InputBase private: void _read_control_data_from_position_setpoint_sub(ControlData &control_data); - int _vehicle_roi_sub = -1; - int _position_setpoint_triplet_sub = -1; + orb_sub_t _vehicle_roi_sub = ORB_SUB_INVALID; + orb_sub_t _position_setpoint_triplet_sub = ORB_SUB_INVALID; uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE}; }; @@ -89,7 +89,7 @@ class InputMavlinkCmdMount : public InputBase UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command); void _ack_vehicle_command(const vehicle_command_s &cmd); - int _vehicle_command_sub = -1; + orb_sub_t _vehicle_command_sub = ORB_SUB_INVALID; }; class InputMavlinkGimbalV2 : public InputBase @@ -118,11 +118,11 @@ class InputMavlinkGimbalV2 : public InputBase void _stream_gimbal_manager_status(const ControlData &control_data); void _read_control_data_from_position_setpoint_sub(ControlData &control_data); - int _vehicle_roi_sub = -1; - int _gimbal_manager_set_attitude_sub = -1; - int _gimbal_manager_set_manual_control_sub = -1; - int _position_setpoint_triplet_sub = -1; - int _vehicle_command_sub = -1; + orb_sub_t _vehicle_roi_sub = ORB_SUB_INVALID; + orb_sub_t _gimbal_manager_set_attitude_sub = ORB_SUB_INVALID; + orb_sub_t _gimbal_manager_set_manual_control_sub = ORB_SUB_INVALID; + orb_sub_t _position_setpoint_triplet_sub = ORB_SUB_INVALID; + orb_sub_t _vehicle_command_sub = ORB_SUB_INVALID; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index e95705ff6641..bebac7475f3c 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -51,7 +51,7 @@ InputRC::InputRC(Parameters ¶meters) : InputRC::~InputRC() { - if (_manual_control_setpoint_sub >= 0) { + if (orb_sub_valid(_manual_control_setpoint_sub)) { orb_unsubscribe(_manual_control_setpoint_sub); } } @@ -60,7 +60,7 @@ int InputRC::initialize() { _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - if (_manual_control_setpoint_sub < 0) { + if (!orb_sub_valid(_manual_control_setpoint_sub)) { return -errno; } diff --git a/src/modules/gimbal/input_rc.h b/src/modules/gimbal/input_rc.h index 2bf0c2f51f05..5483cab6314a 100644 --- a/src/modules/gimbal/input_rc.h +++ b/src/modules/gimbal/input_rc.h @@ -59,7 +59,7 @@ class InputRC : public InputBase virtual UpdateResult _read_control_data_from_subscription(ControlData &control_data, bool already_active); float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx); - int _manual_control_setpoint_sub{-1}; + orb_sub_t _manual_control_setpoint_sub{ORB_SUB_INVALID}; float _last_set_aux_values[3] {}; }; diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 51818c342552..6ea2bd3dc1bd 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -135,6 +135,11 @@ bool GyroFFT::init() buffers_allocated = AllocateBuffers<256>(); _param_imu_gyro_fft_len.set(256); _param_imu_gyro_fft_len.commit(); + _rfft_q15.fftLenReal = 256; + _rfft_q15.twidCoefRModifier = 32U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; + PX4_INFO("Setting IMU_GYRO_FFT_LEN=%" PRId32 ", as default", _param_imu_gyro_fft_len.get()); + break; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 9786a48f468b..cb4ae6617963 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -289,7 +289,7 @@ bool MulticopterLandDetector::_get_landed_state() bool MulticopterLandDetector::_get_ground_effect_state() { - return (_in_descend && !_horizontal_movement) || + return (_below_gnd_effect_hgt && _in_descend && !_horizontal_movement) || (_below_gnd_effect_hgt && _takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT) || _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP; } diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index 80c54f482694..b9ae25f1f19b 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -259,7 +259,8 @@ void LoadMon::stack_usage() if (system_load.tasks[_stack_task_index].valid && (system_load.tasks[_stack_task_index].tcb->pid > 0)) { - stack_free = up_check_tcbstack_remain(system_load.tasks[_stack_task_index].tcb); + stack_free = system_load.tasks[_stack_task_index].tcb->adj_stack_size - up_check_tcbstack( + system_load.tasks[_stack_task_index].tcb); strncpy((char *)task_stack_info.task_name, system_load.tasks[_stack_task_index].tcb->name, CONFIG_TASK_NAME_SIZE - 1); task_stack_info.task_name[CONFIG_TASK_NAME_SIZE - 1] = '\0'; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 508a5427549f..4c42073297bf 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -5,7 +5,7 @@ #include #include -orb_advert_t mavlink_log_pub = nullptr; +orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; // required standard deviation of estimate for estimator to publish data static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index f9f08fddb667..5cd90b2ff791 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -304,7 +304,7 @@ int LogWriterFile::thread_start() param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; (void)pthread_attr_setschedparam(&thr_attr, ¶m); - pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1170)); + pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1370)); int ret = pthread_create(&_thread, &thr_attr, &LogWriterFile::run_helper, this); pthread_attr_destroy(&thr_attr); diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp index 1c2a7b4bc499..5322aef074e0 100644 --- a/src/modules/logger/log_writer_mavlink.cpp +++ b/src/modules/logger/log_writer_mavlink.cpp @@ -57,14 +57,14 @@ bool LogWriterMavlink::init() LogWriterMavlink::~LogWriterMavlink() { - if (_ulog_stream_ack_sub >= 0) { + if (orb_sub_valid(_ulog_stream_ack_sub)) { orb_unsubscribe(_ulog_stream_ack_sub); } } void LogWriterMavlink::start_log() { - if (_ulog_stream_ack_sub == -1) { + if (!orb_sub_valid(_ulog_stream_ack_sub)) { _ulog_stream_ack_sub = orb_subscribe(ORB_ID(ulog_stream_ack)); } diff --git a/src/modules/logger/log_writer_mavlink.h b/src/modules/logger/log_writer_mavlink.h index ad6d80fa3bc6..8aabe2d79235 100644 --- a/src/modules/logger/log_writer_mavlink.h +++ b/src/modules/logger/log_writer_mavlink.h @@ -78,7 +78,7 @@ class LogWriterMavlink ulog_stream_s _ulog_stream_data{}; uORB::Publication _ulog_stream_pub{ORB_ID(ulog_stream)}; - int _ulog_stream_ack_sub{-1}; + orb_sub_t _ulog_stream_ack_sub{ORB_SUB_INVALID}; bool _need_reliable_transfer{false}; bool _is_started{false}; }; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 9e194ca6873b..e66c6f69ddbc 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -651,12 +651,12 @@ void Logger::run() /* timer_semaphore use case is a signal */ px4_sem_setprotocol(&timer_callback_data.semaphore, SEM_PRIO_NONE); - int polling_topic_sub = -1; + orb_sub_t polling_topic_sub = ORB_SUB_INVALID; if (_polling_topic_meta) { polling_topic_sub = orb_subscribe(_polling_topic_meta); - if (polling_topic_sub < 0) { + if (!orb_sub_valid(polling_topic_sub)) { PX4_ERR("Failed to subscribe (%i)", errno); } @@ -679,7 +679,7 @@ void Logger::run() hrt_abstime next_subscribe_check = 0; int next_subscribe_topic_index = -1; // this is used to distribute the checks over time - if (polling_topic_sub >= 0) { + if (orb_sub_valid(polling_topic_sub)) { _lockstep_component = px4_lockstep_register_component(); } @@ -886,7 +886,7 @@ void Logger::run() update_params(); // wait for next loop iteration... - if (polling_topic_sub >= 0) { + if (orb_sub_valid(polling_topic_sub)) { px4_lockstep_progress(_lockstep_component); px4_pollfd_struct_t fds[1]; @@ -927,13 +927,13 @@ void Logger::run() // stop the writer thread _writer.thread_stop(); - if (polling_topic_sub >= 0) { + if (orb_sub_valid(polling_topic_sub)) { orb_unsubscribe(polling_topic_sub); } - if (_mavlink_log_pub) { + if (orb_advert_valid(_mavlink_log_pub)) { orb_unadvertise(_mavlink_log_pub); - _mavlink_log_pub = nullptr; + _mavlink_log_pub = ORB_ADVERT_INVALID; } px4_unregister_shutdown_hook(&Logger::request_stop_static); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index c3b42c258e2c..7f24098032b2 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -104,7 +104,7 @@ void ManualControl::Run() _param_man_arm_gesture.set(0); // disable arm gesture _param_man_arm_gesture.commit(); - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t") /* EVENT * @description MAN_ARM_GESTURE is now set to disable arm/disarm stick gesture. @@ -126,7 +126,7 @@ void ManualControl::Run() airmode = 1; // change to roll/pitch airmode param_set(param_mc_airmode, &airmode); - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t") /* EVENT * @description MC_AIRMODE is now set to roll/pitch airmode. diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 9d078b16b090..5770880a0572 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,7 +34,11 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include #include diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 47082b9098e0..b3470d1c7287 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1876,7 +1876,7 @@ Mavlink::task_main(int argc, char *argv[]) int temp_int_arg; #endif - while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:i:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, _baudrate) != 0) { @@ -1962,8 +1962,6 @@ Mavlink::task_main(int argc, char *argv[]) _mav_broadcast = BROADCAST_MODE_ON; break; -#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) - // multicast case 'c': _src_addr.sin_family = AF_INET; @@ -1978,13 +1976,37 @@ Mavlink::task_main(int argc, char *argv[]) } break; -#else - case 'c': - PX4_ERR("Multicast option is not supported on this platform"); - err_flag = true; + case 'i': { + _src_addr.sin_family = AF_INET; + int mav_id = atoi(myoptarg); + char tmp_str[32]; + uint8_t digits[4] = {0, 0, 0, 0}; + + for (int i = 0; i < 4; i++) { + snprintf(tmp_str, 32, "p:MAV_%d_REMOTE_IP%d", mav_id, i); + int ret = px4_get_parameter_value(tmp_str, temp_int_arg); + + if (!ret) { + digits[i] = (uint8_t)(temp_int_arg & 0xff); + + } else { + PX4_ERR("parse_ip: cant parse %s -> %d", tmp_str, ret); + } + } + + snprintf(tmp_str, 32, "%d.%d.%d.%d", digits[0], digits[1], digits[2], digits[3]); + + if (inet_aton(tmp_str, &_src_addr.sin_addr)) { + PX4_INFO("UDP mavlink remote address: %s", tmp_str); + _src_addr_initialized = true; + + } else { + PX4_ERR("invalid partner ip '%s'", tmp_str); + err_flag = true; + } + } break; -#endif #else case 'p': @@ -2955,10 +2977,8 @@ Mavlink::display_status() printf("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port); printf("\tBroadcast enabled: %s\n", broadcast_enabled() ? "YES" : "NO"); -#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) printf("\tMulticast enabled: %s\n", multicast_enabled() ? "YES" : "NO"); -#endif #ifdef __PX4_POSIX if (get_client_source_initialized()) { @@ -3315,14 +3335,13 @@ Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz: PRINT_MODULE_USAGE_PARAM_FLAG('p', "Enable Broadcast", true); PRINT_MODULE_USAGE_PARAM_INT('u', 14556, 0, 65536, "Select UDP Network Port (local)", true); PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "Partner IP from MAV__REMOTE_IPn params, where is the argument value", true); PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via -p flag)", true); #endif PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal|uavionix", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); -#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_{i}_BROADCAST param)", true); -#endif PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true); PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true); PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f42b0a6e1e5d..3eae40d644c4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -338,7 +338,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: - handle_message_hil_state_quaternion(msg); + /* Do not handle hil_state_quaternion, + but let ekf2 to take care of position + estimation. + */ + //handle_message_hil_state_quaternion(msg); break; case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: @@ -2316,6 +2320,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mavlink_hil_sensor_t hil_sensor; mavlink_msg_hil_sensor_decode(msg, &hil_sensor); + // Workaround for mavlink2 msg generator bug, just in case. + hil_sensor.id = 0; + const uint64_t timestamp = hrt_absolute_time(); // temperature only updated with baro @@ -2408,7 +2415,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; hil_battery_status.remaining = 0.70; - hil_battery_status.time_remaining_s = NAN; + hil_battery_status.time_remaining_s = (float)NAN; _battery_pub.publish(hil_battery_status); } @@ -2733,119 +2740,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } - - /* attitude */ - { - vehicle_attitude_s hil_attitude{}; - hil_attitude.timestamp_sample = timestamp_sample; - matrix::Quatf q(hil_state.attitude_quaternion); - q.copyTo(hil_attitude.q); - hil_attitude.timestamp = hrt_absolute_time(); - _attitude_pub.publish(hil_attitude); - } - - /* global position */ - { - vehicle_global_position_s hil_global_pos{}; - - hil_global_pos.timestamp_sample = timestamp_sample; - hil_global_pos.lat = hil_state.lat / ((double)1e7); - hil_global_pos.lon = hil_state.lon / ((double)1e7); - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.eph = 2.f; - hil_global_pos.epv = 4.f; - hil_global_pos.timestamp = hrt_absolute_time(); - _global_pos_pub.publish(hil_global_pos); - } - - /* local position */ - { - const double lat = hil_state.lat * 1e-7; - const double lon = hil_state.lon * 1e-7; - - if (!_global_local_proj_ref.isInitialized() || !PX4_ISFINITE(_global_local_alt0)) { - _global_local_proj_ref.initReference(lat, lon); - _global_local_alt0 = hil_state.alt / 1000.f; - } - - float x = 0.f; - float y = 0.f; - _global_local_proj_ref.project(lat, lon, x, y); - - vehicle_local_position_s hil_local_pos{}; - hil_local_pos.timestamp_sample = timestamp_sample; - hil_local_pos.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp(); - hil_local_pos.ref_lat = _global_local_proj_ref.getProjectionReferenceLat(); - hil_local_pos.ref_lon = _global_local_proj_ref.getProjectionReferenceLon(); - hil_local_pos.ref_alt = _global_local_alt0; - hil_local_pos.xy_valid = true; - hil_local_pos.z_valid = true; - hil_local_pos.v_xy_valid = true; - hil_local_pos.v_z_valid = true; - hil_local_pos.x = x; - hil_local_pos.y = y; - hil_local_pos.z = _global_local_alt0 - hil_state.alt / 1000.f; - hil_local_pos.vx = hil_state.vx / 100.f; - hil_local_pos.vy = hil_state.vy / 100.f; - hil_local_pos.vz = hil_state.vz / 100.f; - - matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; - hil_local_pos.heading = euler.psi(); - hil_local_pos.xy_global = true; - hil_local_pos.z_global = true; - hil_local_pos.vxy_max = INFINITY; - hil_local_pos.vz_max = INFINITY; - hil_local_pos.hagl_min = INFINITY; - hil_local_pos.hagl_max = INFINITY; - hil_local_pos.timestamp = hrt_absolute_time(); - _local_pos_pub.publish(hil_local_pos); - } - - /* accelerometer */ - { - if (_px4_accel == nullptr) { - // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - _px4_accel = new PX4Accelerometer(1310988); - - if (_px4_accel == nullptr) { - PX4_ERR("PX4Accelerometer alloc failed"); - } - } - - if (_px4_accel != nullptr) { - // accel in mG - _px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f); - _px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc); - } - } - - /* gyroscope */ - { - if (_px4_gyro == nullptr) { - // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - _px4_gyro = new PX4Gyroscope(1310988); - - if (_px4_gyro == nullptr) { - PX4_ERR("PX4Gyroscope alloc failed"); - } - } - - if (_px4_gyro != nullptr) { - _px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed); - } - } - - /* battery status */ - { - battery_status_s hil_battery_status{}; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; - hil_battery_status.current_a = 10.0f; - hil_battery_status.discharged_mah = -1.0f; - hil_battery_status.timestamp = hrt_absolute_time(); - hil_battery_status.time_remaining_s = NAN; - _battery_pub.publish(hil_battery_status); - } } #if !defined(CONSTRAINED_FLASH) diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index cce39a472fb2..a371b33a95ee 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -146,7 +146,7 @@ int MavlinkShell::start() _task = px4_task_spawn_cmd("mavlink_shell", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + PX4_STACK_ADJUSTED(2048), &MavlinkShell::shell_start_thread, #ifdef __PX4_POSIX argv); diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index 6bfe8b23a590..dba609cdfb6e 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -15,6 +15,26 @@ serial_config: then set MAV_ARGS "${MAV_ARGS} -c" fi + set USE_REMOTE_IP 0 + if param greater MAV_${i}_REMOTE_IP0 0 + then + set USE_REMOTE_IP 1 + fi + if param greater MAV_${i}_REMOTE_IP1 0 + then + set USE_REMOTE_IP 1 + fi + if param greater MAV_${i}_REMOTE_IP2 0 + then + set USE_REMOTE_IP 1 + fi + if param greater MAV_${i}_REMOTE_IP3 0 + then + set USE_REMOTE_IP 1 + fi + if [ ${USE_REMOTE_IP} != 0 ]; then + set MAV_ARGS "${MAV_ARGS} -i ${i}" + fi fi if param compare MAV_${i}_FORWARD 1 then @@ -177,3 +197,63 @@ parameters: 2: Auto-detected num_instances: *max_num_config_instances default: [2, 2, 2] + + MAV_${i}_REMOTE_IP0: + description: + short: MAVLink 1st digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 1st digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_IP1: + description: + short: MAVLink 2nd digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 2nd digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_IP2: + description: + short: MAVLink 3rd digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 3rd digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_IP3: + description: + short: MAVLink 4th digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 4th digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true diff --git a/src/modules/microdds_client/CMakeLists.txt b/src/modules/microdds_client/CMakeLists.txt index 8860267f7927..be47253ce2de 100644 --- a/src/modules/microdds_client/CMakeLists.txt +++ b/src/modules/microdds_client/CMakeLists.txt @@ -117,6 +117,7 @@ else() add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py + --camel-case-topic-names --topic-msg-dir ${PX4_SOURCE_DIR}/msg --client-outdir ${CMAKE_CURRENT_BINARY_DIR} --dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml diff --git a/src/modules/microdds_client/Kconfig b/src/modules/microdds_client/Kconfig index df77443e1433..a27d0e6f13c7 100644 --- a/src/modules/microdds_client/Kconfig +++ b/src/modules/microdds_client/Kconfig @@ -3,3 +3,10 @@ menuconfig MODULES_MICRODDS_CLIENT default n ---help--- Enable support for the MicroDDS Client + +menuconfig USER_MICRODDS_CLIENT + bool "microdds_client running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_MICRODDS_CLIENT + ---help--- + Put microdds_client in userspace memory diff --git a/src/modules/microdds_client/Micro-XRCE-DDS-Client b/src/modules/microdds_client/Micro-XRCE-DDS-Client index 4248559f3b11..d44682c0e065 160000 --- a/src/modules/microdds_client/Micro-XRCE-DDS-Client +++ b/src/modules/microdds_client/Micro-XRCE-DDS-Client @@ -1 +1 @@ -Subproject commit 4248559f3b111155c783e524e461ccc83e768103 +Subproject commit d44682c0e065dc5d35d04d053ee211fed3130738 diff --git a/src/modules/microdds_client/dds_topics.h.em b/src/modules/microdds_client/dds_topics.h.em index bde8b8d8cbfd..cfefba1115cb 100644 --- a/src/modules/microdds_client/dds_topics.h.em +++ b/src/modules/microdds_client/dds_topics.h.em @@ -49,29 +49,38 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream if (@(pub['topic_simple'])_sub.update(&data)) { - if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { - // data writer not created yet - create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); - } +@[ if pub['topic_pub_thold_us'] > 0 ]@ + // Rate control + static uint64_t @(pub['topic_simple'])_prev_timestamp = 0; + if (@(pub['topic_simple'])_prev_timestamp == 0) @(pub['topic_simple'])_prev_timestamp = data.timestamp; + if (data.timestamp - @(pub['topic_simple'])_prev_timestamp >= @(pub['topic_pub_thold_us'])) { + @(pub['topic_simple'])_prev_timestamp = data.timestamp; +@[ end if ]@ + if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { + // data writer not created yet + create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_name'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); + } + + if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { - if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { + ucdrBuffer ub; + uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); + if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { + ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); + // TODO: fill up the MTU and then flush, which reduces the packet overhead + uxr_flash_output_streams(session); + num_payload_sent += topic_size; - ucdrBuffer ub; - uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); - if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { - ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); - // TODO: fill up the MTU and then flush, which reduces the packet overhead - uxr_flash_output_streams(session); - num_payload_sent += topic_size; + } else { + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); + } } else { - //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); + //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); } - - } else { - //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); - } - +@[ if pub['topic_pub_thold_us'] > 0 ]@ + } // Rate control +@[ end if ]@ } } @[ end for]@ @@ -118,7 +127,7 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId reliable_in_stream_id, uxrStreamId best_effort_in_stream_id, uxrObjectId participant_id, const char *client_namespace) { @[ for idx, sub in enumerate(subscriptions)]@ - create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])"); + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_name'])", "@(sub['dds_type'])"); @[ end for]@ uxr_set_topic_callback(session, on_topic_update, this); diff --git a/src/modules/microdds_client/dds_topics.yaml b/src/modules/microdds_client/dds_topics.yaml index 3a871848bb23..2bb0488c3af6 100644 --- a/src/modules/microdds_client/dds_topics.yaml +++ b/src/modules/microdds_client/dds_topics.yaml @@ -44,6 +44,29 @@ publications: - topic: /fmu/out/vehicle_trajectory_waypoint_desired type: px4_msgs::msg::VehicleTrajectoryWaypoint +# messages added by ssrc + + - topic: /fmu/out/battery_status + type: px4_msgs::msg::BatteryStatus + + - topic: /fmu/out/distance_sensor + type: px4_msgs::msg::DistanceSensor + + - topic: /fmu/out/mission_result + type: px4_msgs::msg::MissionResult + + - topic: /fmu/out/sensor_baro + type: px4_msgs::msg::SensorBaro + + - topic: /fmu/out/sensor_mag + type: px4_msgs::msg::SensorMag + + - topic: /fmu/out/vehicle_land_detected + type: px4_msgs::msg::VehicleLandDetected + + - topic: /fmu/out/home_position + type: px4_msgs::msg::HomePosition + subscriptions: - topic: /fmu/in/offboard_control_mode @@ -84,3 +107,11 @@ subscriptions: - topic: /fmu/in/vehicle_trajectory_waypoint type: px4_msgs::msg::VehicleTrajectoryWaypoint + +# messages added by ssrc + + - topic: /fmu/in/sensor_gps + type: px4_msgs::msg::SensorGps + + - topic: /fmu/in/gps_inject_data + type: px4_msgs::msg::GpsInjectData diff --git a/src/modules/microdds_client/generate_dds_topics.py b/src/modules/microdds_client/generate_dds_topics.py index 8018fe22d0bf..24a7092d1b1f 100644 --- a/src/modules/microdds_client/generate_dds_topics.py +++ b/src/modules/microdds_client/generate_dds_topics.py @@ -43,6 +43,9 @@ import yaml parser = argparse.ArgumentParser() +parser.add_argument("-c", "--camel-case-topic-names", dest='camelcase', action='store_true', + help="Use CamelCase topic names instead of snake_case") + parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, help="Topics message, by default using relative path 'msg/'", default="msg") @@ -102,6 +105,20 @@ # topic_simple: eg vehicle_status p['topic_simple'] = p['topic'].split('/')[-1] + if args.camelcase: + simple_topic_name = p['topic_simple'] + topic_name_camel_case = ''.join(part.title() for part in simple_topic_name.split('_')) + p['topic_name'] = topic_name_camel_case + else: + p['topic_name'] = p['topic_simple'] + + # topic_pub_thold: eg. rate: 10Hz => topic_pub_thold_us = 100000 (us) + if 'rate' in p: + p['topic_pub_thold_us'] = int(1000000 / p['rate']) + print(p['topic_name'] + " rate: " + str(p['rate']) + " => thold_us: " + str(p['topic_pub_thold_us'])) + else: + print(p['topic_name'] + " rate: N/A") + p['topic_pub_thold_us'] = 0 merged_em_globals['publications'] = msg_map['publications'] @@ -122,6 +139,13 @@ # topic_simple: eg vehicle_status s['topic_simple'] = s['topic'].split('/')[-1] + if args.camelcase: + simple_topic_name = s['topic_simple'] + topic_name_camel_case = ''.join(part.title() for part in simple_topic_name.split('_')) + s['topic_name'] = topic_name_camel_case + else: + s['topic_name'] = s['topic_simple'] + merged_em_globals['subscriptions'] = msg_map['subscriptions'] diff --git a/src/modules/microdds_client/microdds_client.cpp b/src/modules/microdds_client/microdds_client.cpp index 1c371da8cce4..eb8665455929 100644 --- a/src/modules/microdds_client/microdds_client.cpp +++ b/src/modules/microdds_client/microdds_client.cpp @@ -80,7 +80,7 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta } MicroddsClient::MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, - const char *port, bool localhost_only, const char *client_namespace) : + const char *recv_port, const char *send_port, bool localhost_only, const char *client_namespace) : ModuleParams(nullptr), _localhost_only(localhost_only), _client_namespace(client_namespace) @@ -124,7 +124,7 @@ MicroddsClient::MicroddsClient(Transport transport, const char *device, int baud _transport_udp = new uxrUDPTransport(); if (_transport_udp) { - if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, host, port)) { + if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, host, recv_port, send_port)) { _comm = &_transport_udp->comm; _fd = _transport_udp->platform.poll_fd.fd; @@ -172,6 +172,7 @@ void MicroddsClient::run() while (!should_exit()) { bool got_response = false; + PX4_INFO("Waiting for ping response..."); while (!should_exit() && !got_response) { // Sending ping without initing a XRCE session @@ -197,7 +198,7 @@ void MicroddsClient::run() // void uxr_create_session_retries(uxrSession* session, size_t retries); if (!uxr_create_session(&session)) { PX4_ERR("uxr_create_session failed"); - return; + continue; } // TODO: uxr_set_status_callback @@ -230,59 +231,20 @@ void MicroddsClient::run() // uint16_t participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, domain_id, // participant_name, UXR_REPLACE); - char participant_xml[PARTICIPANT_XML_SIZE]; - int ret = snprintf(participant_xml, PARTICIPANT_XML_SIZE, "%s%s/px4_micro_xrce_dds%s", - _localhost_only ? - "" - "" - "" - "" - "udp_localhost" - "UDPv4" - "
127.0.0.1
" - "" - "" - "" - "" - "" - : - "" - "" - "", - _client_namespace != nullptr ? - _client_namespace - : - "", - _localhost_only ? - "false" - "udp_localhost" - "" - "" - "" - : - "" - "" - "" - ); - - if (ret < 0 || ret >= TOPIC_NAME_SIZE) { - PX4_ERR("create entities failed: namespace too long"); - return; - } - - uint16_t participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id, - participant_xml, UXR_REPLACE); + const char *participant_ref = "default_xrce_participant"; + uint16_t participant_req = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id, domain_id, + participant_ref, UXR_REPLACE); uint8_t request_status; if (!uxr_run_session_until_all_status(&session, 1000, &participant_req, &request_status, 1)) { PX4_ERR("create entities failed: participant: %i", request_status); - return; + continue; } if (!_pubs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) { PX4_ERR("pubs init failed"); - return; + continue; } _connected = true; @@ -371,6 +333,21 @@ void MicroddsClient::run() uxr_delete_session_retries(&session, _connected ? 1 : 0); _last_payload_tx_rate = 0; _last_payload_tx_rate = 0; + + if (_subs) { + delete _subs; + } + _subs = new SendTopicsSubs(); + + if (_pubs) { + delete _pubs; + } + _pubs = new RcvTopicsPubs(); + + if (!_subs || !_pubs) { + PX4_ERR("alloc failed"); + return; + } } } @@ -557,13 +534,14 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) const char *device = nullptr; int baudrate = 921600; - const char *port = "8888"; + const char *recv_port = "0"; + const char *send_port = "8888"; bool localhost_only = false; const char *ip = "127.0.0.1"; const char *client_namespace = nullptr;//"px4"; - while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:l:n:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:r:l:n:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 't': if (!strcmp(myoptarg, "serial")) { @@ -598,7 +576,11 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) break; case 'p': - port = myoptarg; + send_port = myoptarg; + break; + + case 'r': + recv_port = myoptarg; break; case 'l': @@ -632,7 +614,7 @@ MicroddsClient *MicroddsClient::instantiate(int argc, char *argv[]) } } - return new MicroddsClient(transport, device, baudrate, ip, port, localhost_only, client_namespace); + return new MicroddsClient(transport, device, baudrate, ip, recv_port, send_port, localhost_only, client_namespace); } int MicroddsClient::print_usage(const char *reason) @@ -658,6 +640,7 @@ MicroDDS Client used to communicate uORB topics with an Agent over serial or UDP PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:)", true); PRINT_MODULE_USAGE_PARAM_STRING('h', "127.0.0.1", "", "Host IP", true); PRINT_MODULE_USAGE_PARAM_INT('p', 8888, 0, 65535, "Remote Port", true); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 65536, "Local Port", true); PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/microdds_client/microdds_client.h b/src/modules/microdds_client/microdds_client.h index 6403195c1ac0..4d9d936dcd92 100644 --- a/src/modules/microdds_client/microdds_client.h +++ b/src/modules/microdds_client/microdds_client.h @@ -48,8 +48,8 @@ class MicroddsClient : public ModuleBase, public ModuleParams Udp }; - MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port, - bool localhost_only, const char *client_namespace); + MicroddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *recv_port, + const char *send_port, bool localhost_only, const char *client_namespace); ~MicroddsClient(); diff --git a/src/modules/microdds_client/utilities.hpp b/src/modules/microdds_client/utilities.hpp index c5eb5c3a2677..8291cbde0bf1 100644 --- a/src/modules/microdds_client/utilities.hpp +++ b/src/modules/microdds_client/utilities.hpp @@ -54,23 +54,39 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str // data writer + char datawriter_xml[1024]; datawriter_id = uxr_object_id(topic_id.id, UXR_DATAWRITER_ID); + const char *datawriter_xml_format = "" + "" + "" + "NO_KEY" + "%s" + "px4_msgs::msg::dds_::%s_" + "" + "" + "" + "ASYNCHRONOUS" + "" + "" + "PREALLOCATED_WITH_REALLOC" + "" + ""; + + int ret = snprintf(datawriter_xml, 1024, datawriter_xml_format, topic_name, topic_name_simple); + + if (ret < 0) { + PX4_ERR("Can't create datawriter_xml string"); + return false; + } - uxrQoS_t qos = { - .durability = UXR_DURABILITY_TRANSIENT_LOCAL, - .reliability = UXR_RELIABILITY_BEST_EFFORT, - .history = UXR_HISTORY_KEEP_LAST, - .depth = 0, - }; - - uint16_t datawriter_req = uxr_buffer_create_datawriter_bin(session, reliable_out_stream_id, datawriter_id, publisher_id, - topic_id, qos, UXR_REPLACE); + uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(session, reliable_out_stream_id, datawriter_id, publisher_id, + datawriter_xml, UXR_REPLACE); // Send create entities message and wait its status uint16_t requests[3] {topic_req, publisher_req, datawriter_req}; uint8_t status[3]; - if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + if (!uxr_run_session_until_all_status(session, 5000, requests, status, 3)) { PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", topic_name, status[0], status[1], status[2]); return false; @@ -109,22 +125,33 @@ static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_str // data reader + char datareader_xml[1024]; uxrObjectId datareader_id = uxr_object_id(id, UXR_DATAREADER_ID); + const char *datareader_xml_format = "" + "" + "" + "NO_KEY" + "%s" + "px4_msgs::msg::dds_::%s_" + "" + "PREALLOCATED_WITH_REALLOC" + "" + ""; + + int ret = snprintf(datareader_xml, 1024, datareader_xml_format, topic_name, topic_name_simple); + + if (ret < 0) { + PX4_ERR("Can't create datareader_xml string"); + return false; + } - uxrQoS_t qos = { - .durability = UXR_DURABILITY_VOLATILE, - .reliability = UXR_RELIABILITY_BEST_EFFORT, - .history = UXR_HISTORY_KEEP_LAST, - .depth = 0, - }; - - uint16_t datareader_req = uxr_buffer_create_datareader_bin(session, reliable_out_stream_id, datareader_id, - subscriber_id, topic_id, qos, UXR_REPLACE); + uint16_t datareader_req = uxr_buffer_create_datareader_xml(session, reliable_out_stream_id, datareader_id, + subscriber_id, datareader_xml, UXR_REPLACE); uint16_t requests[3] {topic_req, subscriber_req, datareader_req}; uint8_t status[3]; - if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + if (!uxr_run_session_until_all_status(session, 5000, requests, status, 3)) { PX4_ERR("create entities failed: %s %i %i %i", topic_name, status[0], status[1], status[2]); return false; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index dcff5643d5fd..72ce336f0e34 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -330,9 +330,9 @@ class Navigator : public ModuleBase, public ModuleParams hrt_abstime timestamp; }; - int _local_pos_sub{-1}; - int _mission_sub{-1}; - int _vehicle_status_sub{-1}; + orb_sub_t _local_pos_sub{ORB_SUB_INVALID}; + orb_sub_t _mission_sub{ORB_SUB_INVALID}; + orb_sub_t _vehicle_status_sub{ORB_SUB_INVALID}; uORB::SubscriptionData _position_controller_status_sub{ORB_ID(position_controller_status)}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3a561083cf78..2c1c1415a9c1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -949,7 +949,7 @@ int Navigator::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_NAVIGATION, - PX4_STACK_ADJUSTED(1952), + PX4_STACK_ADJUSTED(2300), (px4_main_t)&run_trampoline, (char *const *)argv); diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index e91de4554d83..9684546c55b5 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -933,9 +933,9 @@ Replay::run() subscription->compat = nullptr; } - if (subscription->orb_advert) { + if (orb_advert_valid(subscription->orb_advert)) { orb_unadvertise(subscription->orb_advert); - subscription->orb_advert = nullptr; + subscription->orb_advert = ORB_ADVERT_INVALID; } } @@ -1015,8 +1015,8 @@ Replay::publishTopic(Subscription &sub, void *data) data = sub.compat->apply(data); } - if (sub.orb_advert) { - orb_publish(sub.orb_meta, sub.orb_advert, data); + if (orb_advert_valid(sub.orb_advert)) { + orb_publish(sub.orb_meta, &sub.orb_advert, data); published = true; } else { @@ -1035,7 +1035,7 @@ Replay::publishTopic(Subscription &sub, void *data) if (subscription->orb_meta) { if (strcmp(sub.orb_meta->o_name, subscription->orb_meta->o_name) == 0 && - subscription->orb_advert && subscription->multi_id == sub.multi_id - 1) { + orb_advert_valid(subscription->orb_advert) && subscription->multi_id == sub.multi_id - 1) { advertised = true; } } diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 94ecc9d416fd..ea32a720a5ad 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -128,7 +128,7 @@ class Replay : public ModuleBase struct Subscription { const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid - orb_advert_t orb_advert = nullptr; + orb_advert_t orb_advert = ORB_ADVERT_INVALID; uint8_t multi_id; int timestamp_offset; ///< marks the field of the timestamp diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 71970b949051..511619395735 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -58,6 +58,7 @@ #include +static int _tcp_server_fd; static int _fd; static unsigned char _buf[2048]; static sockaddr_in _srcaddr; @@ -371,7 +372,11 @@ void SimulatorMavlink::handle_message(const mavlink_message_t *msg) break; case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: - handle_message_hil_state_quaternion(msg); + /* Do not handle hil_state_quaternion, + but let ekf2 to take care of position + estimation. + */ + //handle_message_hil_state_quaternion(msg); break; case MAVLINK_MSG_ID_RAW_RPM: @@ -442,6 +447,7 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) // New publishers will be created based on the HIL_GPS ID's being different or not for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) { + gps.device_id = _gps_dev_ids[i]; _sensor_gps_pubs[i]->publish(gps); break; } @@ -456,6 +462,7 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) device_id.devid_s.address = i; device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; gps.device_id = device_id.devid; + _gps_dev_ids[i] = device_id.devid; _sensor_gps_pubs[i]->publish(gps); break; @@ -469,6 +476,9 @@ void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); + // Workaround for mavlink2 msg generator bug + imu.id = 0; + // Assume imu with id 0 is the primary imu an base lockstep based on this. if (imu.id == 0) { if (_lockstep_component == -1) { @@ -1084,6 +1094,7 @@ void SimulatorMavlink::run() if (_ip == InternetProtocol::UDP) { + // UDP 'server' mode if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { PX4_ERR("Creating UDP socket failed: %s", strerror(errno)); return; @@ -1112,8 +1123,80 @@ void SimulatorMavlink::run() PX4_INFO("Simulator connected on UDP port %u.", _port); + } else if (_server_mode) { + + // TCP Server mode + PX4_INFO("TCP Server: Waiting for simulator to connect on TCP port %u", _port); + + if ((_tcp_server_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + PX4_ERR("TCP Server: Creating TCP socket failed: %s", strerror(errno)); + return; + } + + int yes = 1; + int ret = setsockopt(_tcp_server_fd, IPPROTO_TCP, TCP_NODELAY, (char *) &yes, sizeof(int)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + struct linger nolinger {}; + + nolinger.l_onoff = 1; + + nolinger.l_linger = 0; + + ret = setsockopt(_tcp_server_fd, SOL_SOCKET, SO_LINGER, &nolinger, sizeof(nolinger)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + // The socket reuse is necessary for reconnecting to the same address + // if the socket does not close but gets stuck in TIME_WAIT. This can happen + // if the server is suddenly closed. + int socket_reuse = 1; + ret = setsockopt(_tcp_server_fd, SOL_SOCKET, SO_REUSEADDR, &socket_reuse, sizeof(socket_reuse)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + // Same as above but for a given port + ret = setsockopt(_tcp_server_fd, SOL_SOCKET, SO_REUSEPORT, &socket_reuse, sizeof(socket_reuse)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + socklen_t myaddr_len = sizeof(_myaddr); + + if (bind(_tcp_server_fd, (struct sockaddr *)&_myaddr, myaddr_len) < 0) { + PX4_ERR("TCP Server: Bind for TCP port %u failed: %s", _port, strerror(errno)); + ::close(_tcp_server_fd); + return; + } + + if (listen(_tcp_server_fd, 0) < 0) { + PX4_ERR("TCP Server: listen failed: %s", strerror(errno)); + } + + while (true) { + _fd = accept(_tcp_server_fd, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen); + + if (_fd >= 0) { + break; + } + + PX4_WARN("TCP Server: Accepting client connection failed: %s", strerror(errno)); + } + + PX4_INFO("TCP Server: Accepting TCP client connection from %s:%u", inet_ntoa(_srcaddr.sin_addr), + ntohs(_srcaddr.sin_port)); + } else { + // TCP Client mode PX4_INFO("Waiting for simulator to accept connection on TCP port %u", _port); while (true) { @@ -1557,6 +1640,12 @@ int SimulatorMavlink::start(int argc, char *argv[]) _instance->set_port(atoi(argv[4])); } + if (argc == 5 && strcmp(argv[3], "-s") == 0) { + _instance->set_ip(InternetProtocol::TCP); + _instance->set_port(atoi(argv[4])); + _instance->set_server_mode(); + } + if (argc == 6 && strcmp(argv[3], "-t") == 0) { PX4_INFO("using TCP on remote host %s port %s", argv[4], argv[5]); PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index ee4118a302d1..dee8e88e83b6 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -129,6 +129,7 @@ class SimulatorMavlink : public ModuleParams void set_port(unsigned port) { _port = port; } void set_hostname(const char *hostname) { _hostname = hostname; } void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; } + void set_server_mode() { _server_mode = true; } #if defined(ENABLE_LOCKSTEP_SCHEDULER) bool has_initialized() { return _has_initialized.load(); } @@ -212,6 +213,8 @@ class SimulatorMavlink : public ModuleParams std::string _hostname{""}; + bool _server_mode{false}; + char *_tcp_remote_ipaddr{nullptr}; double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time @@ -260,10 +263,11 @@ class SimulatorMavlink : public ModuleParams static constexpr int MAX_GPS = 3; uORB::PublicationMulti *_sensor_gps_pubs[MAX_GPS] {}; uint8_t _gps_ids[MAX_GPS] {}; + uint32_t _gps_dev_ids[MAX_GPS] {}; std::default_random_engine _gen{}; // uORB subscription handlers - int _actuator_outputs_sub{-1}; + orb_sub_t _actuator_outputs_sub{ORB_SUB_INVALID}; actuator_outputs_s _actuator_outputs{}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake index 37acf693f0b6..51f3dea42f78 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo.cmake @@ -87,6 +87,7 @@ set(models r1_rover rover shell + ssrc_fog_x standard_vtol standard_vtol_drop tailsitter diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.cpp b/src/modules/temperature_compensation/temperature_calibration/accel.cpp index 9dc6c071f86f..f5328d705caa 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/accel.cpp @@ -68,7 +68,7 @@ TemperatureCalibrationAccel::~TemperatureCalibrationAccel() } } -int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub) +int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) { bool finished = data.hot_soaked; diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.h b/src/modules/temperature_compensation/temperature_calibration/accel.h index f464bb0cb564..02bac51e3bfc 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.h +++ b/src/modules/temperature_compensation/temperature_calibration/accel.h @@ -49,7 +49,7 @@ class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3> private: - virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual inline int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.cpp b/src/modules/temperature_compensation/temperature_calibration/baro.cpp index 4d1dc7be9050..3b573a3aee52 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/baro.cpp @@ -68,7 +68,7 @@ TemperatureCalibrationBaro::~TemperatureCalibrationBaro() } } -int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub) +int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) { bool finished = data.hot_soaked; diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.h b/src/modules/temperature_compensation/temperature_calibration/baro.h index fc74e3033da4..226b7a7d77cd 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.h +++ b/src/modules/temperature_compensation/temperature_calibration/baro.h @@ -52,7 +52,7 @@ class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFI private: - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/src/modules/temperature_compensation/temperature_calibration/common.h b/src/modules/temperature_compensation/temperature_calibration/common.h index 442ed01af310..dae2ee1f4580 100644 --- a/src/modules/temperature_compensation/temperature_calibration/common.h +++ b/src/modules/temperature_compensation/temperature_calibration/common.h @@ -188,8 +188,8 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase * update a single sensor instance * @return 0 when done, 1 not finished yet, <0 for an error */ - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0; + virtual int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) = 0; unsigned _num_sensor_instances{0}; - int _sensor_subs[SENSOR_COUNT_MAX]; + orb_sub_t _sensor_subs[SENSOR_COUNT_MAX]; }; diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp index 0f0f9c4a1efc..e75637333765 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp @@ -45,7 +45,7 @@ #include TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, - float max_start_temperature, int gyro_subs[], int num_gyros) + float max_start_temperature, orb_sub_t gyro_subs[], int num_gyros) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { for (int i = 0; i < num_gyros; ++i) { @@ -55,7 +55,7 @@ TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_ris _num_sensor_instances = num_gyros; } -int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub) +int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) { bool finished = data.hot_soaked; diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.h b/src/modules/temperature_compensation/temperature_calibration/gyro.h index c6f9a5df47f3..f64e5cd0a939 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.h +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.h @@ -40,7 +40,7 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { public: TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, - int gyro_subs[], int num_gyros); + orb_sub_t gyro_subs[], int num_gyros); virtual ~TemperatureCalibrationGyro() {} /** @@ -50,7 +50,7 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> private: - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index cb5d0112d8d5..bcf6c0b79f79 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -100,7 +100,7 @@ class TemperatureCalibration void TemperatureCalibration::task_main() { // subscribe to all gyro instances - int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1}; + orb_sub_t gyro_sub[SENSOR_COUNT_MAX] {ORB_SUB_INVALID, ORB_SUB_INVALID, ORB_SUB_INVALID}; px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {}; unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro)); diff --git a/src/systemcmds/mtd/Kconfig b/src/systemcmds/mtd/Kconfig index 661e8ee7f68e..d303b34427c7 100644 --- a/src/systemcmds/mtd/Kconfig +++ b/src/systemcmds/mtd/Kconfig @@ -6,7 +6,7 @@ menuconfig SYSTEMCMDS_MTD menuconfig USER_MTD bool "mtd running as userspace module" - default y + default n depends on BOARD_PROTECTED && SYSTEMCMDS_MTD ---help--- Put mtd in userspace memory diff --git a/src/systemcmds/netconfig/CMakeLists.txt b/src/systemcmds/netconfig/CMakeLists.txt new file mode 100644 index 000000000000..401f78659d0c --- /dev/null +++ b/src/systemcmds/netconfig/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2023 Technology Innovation Institute. 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. +# +############################################################################ +px4_add_module( + MODULE modules__netconfig + MAIN netconfig + SRCS + netconfig.cpp + ) diff --git a/src/systemcmds/netconfig/Kconfig b/src/systemcmds/netconfig/Kconfig new file mode 100644 index 000000000000..8074d6051d24 --- /dev/null +++ b/src/systemcmds/netconfig/Kconfig @@ -0,0 +1,12 @@ +menuconfig SYSTEMCMDS_NETCONFIG + bool "netconfig" + default n + ---help--- + Enable support for netconfig + +menuconfig USER_NETCONFIG + bool "netconfig running as userspace module" + default y + depends on BOARD_PROTECTED && SYSTEMCMDS_NETCONFIG + ---help--- + Put netconfig in userspace memory diff --git a/src/systemcmds/netconfig/netconfig.cpp b/src/systemcmds/netconfig/netconfig.cpp new file mode 100644 index 000000000000..61035722f87d --- /dev/null +++ b/src/systemcmds/netconfig/netconfig.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. 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 netconfig.cpp + * Simple network configuration + * + * @author Jukka Laitinen + */ + +#include +#include +#include +#include +#include + +__BEGIN_DECLS +__EXPORT int netconfig_main(int argc, char *argv[]); +__END_DECLS + +int netconfig_main(int argc, char *argv[]) +{ + struct in_addr addr; + int mav_id; + const char *ifname = argv[1]; + + if (argc != 2) { + return PX4_ERROR; + } + + param_get(param_find("MAV_SYS_ID"), &mav_id); + + if (mav_id < 1) { + return PX4_ERROR; + } + + /* IP: 192.168.201.100 + mav_id */ + + addr.s_addr = 0x00c9a8c0; + + mav_id += 100; + + if (mav_id > 253) { + return PX4_ERROR; + } + + addr.s_addr |= ((uint32_t)mav_id << 24); + netlib_set_ipv4addr(ifname, &addr); + + /* GW: 192.168.201.1 */ + + addr.s_addr = 0x01c9a8c0; + netlib_set_dripv4addr(ifname, &addr); + + /* netmask: 255.255.255.0 */ + + addr.s_addr = 0x00ffffff; + netlib_set_ipv4netmask(ifname, &addr); + + netlib_ifup(ifname); + + return PX4_OK; +} diff --git a/src/systemcmds/tests/test_rc.cpp b/src/systemcmds/tests/test_rc.cpp index 97f70651152f..c9a4beb73050 100644 --- a/src/systemcmds/tests/test_rc.cpp +++ b/src/systemcmds/tests/test_rc.cpp @@ -37,6 +37,7 @@ */ #include +#include #include @@ -59,7 +60,7 @@ int test_rc(int argc, char *argv[]) { - int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_sub_t _rc_sub = orb_subscribe(ORB_ID(input_rc)); /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct input_rc_s rc_input; @@ -85,7 +86,7 @@ int test_rc(int argc, char *argv[]) rc_last.channel_count = rc_input.channel_count; /* poll descriptor */ - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; fds[0].fd = _rc_sub; fds[0].events = POLLIN; fds[1].fd = 0; @@ -93,7 +94,7 @@ int test_rc(int argc, char *argv[]) while (true) { - int ret = poll(fds, 2, 200); + int ret = px4_poll(fds, 2, 200); if (ret > 0) { diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index b4db038c1254..4534406fa6f9 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -39,6 +39,7 @@ #include #include +#include #include @@ -68,7 +69,7 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, if (instances == 1) { PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name); - int sub = orb_subscribe(id); + orb_sub_t sub = orb_subscribe(id); listener_print_topic(id, sub); orb_unsubscribe(sub); @@ -78,7 +79,7 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (orb_exists(id, i) == PX4_OK) { PX4_INFO_RAW("\nInstance %d:\n", i); - int sub = orb_subscribe_multi(id, i); + orb_sub_t sub = orb_subscribe_multi(id, i); listener_print_topic(id, sub); orb_unsubscribe(sub); } @@ -100,55 +101,51 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, return; } - int sub = orb_subscribe_multi(id, topic_instance); + orb_sub_t sub = orb_subscribe_multi(id, topic_instance); orb_set_interval(sub, topic_interval); unsigned msgs_received = 0; - struct pollfd fds[2] {}; - // Poll for user input (for q or escape) - fds[0].fd = 0; /* stdin */ - fds[0].events = POLLIN; + px4_pollfd_struct_t fds[1] {}; // Poll the UOrb subscription - fds[1].fd = sub; - fds[1].events = POLLIN; + fds[0].fd = sub; + fds[0].events = POLLIN; - while (msgs_received < num_msgs) { + int time = 0; - if (poll(&fds[0], 2, int(MESSAGE_TIMEOUT_S * 1000)) > 0) { + while (msgs_received < num_msgs) { - // Received character from stdin - if (fds[0].revents & POLLIN) { - char c = 0; - int ret = read(0, &c, 1); + char c = 0; + int ret = read(0, &c, 1); - if (ret) { - return; - } + if (ret) { - switch (c) { - case 0x03: // ctrl-c - case 0x1b: // esc - case 'q': - return; - /* not reached */ - } + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'q': + return; + /* not reached */ } + } + if (px4_poll(&fds[0], 1, 50) > 0) { // Received message from subscription - if (fds[1].revents & POLLIN) { + + if (fds[0].revents & POLLIN) { msgs_received++; PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received); - int ret = listener_print_topic(id, sub); + ret = listener_print_topic(id, sub); if (ret != PX4_OK) { PX4_ERR("listener callback failed (%i)", ret); } } + } - } else { + if (time += 50 > MESSAGE_TIMEOUT_S * 1000) { PX4_INFO_RAW("Waited for %.1f seconds without a message. Giving up.\n", (double) MESSAGE_TIMEOUT_S); break; } diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp index 1131f4276fea..dda3816501b4 100644 --- a/src/systemcmds/topic_listener/topic_listener.hpp +++ b/src/systemcmds/topic_listener/topic_listener.hpp @@ -50,7 +50,7 @@ #include #include -inline int listener_print_topic(const orb_id_t &orb_id, int subscription) +inline int listener_print_topic(const orb_id_t &orb_id, orb_sub_t subscription) { static constexpr int max_size = 512; alignas(8) char container[max_size]; diff --git a/src/systemcmds/uorb/CMakeLists.txt b/src/systemcmds/uorb/CMakeLists.txt index 2c1362394b47..238c0059a956 100644 --- a/src/systemcmds/uorb/CMakeLists.txt +++ b/src/systemcmds/uorb/CMakeLists.txt @@ -36,5 +36,6 @@ px4_add_module( COMPILE_FLAGS SRCS uorb.cpp + uORBDeviceMaster.cpp DEPENDS ) diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/src/systemcmds/uorb/uORBDeviceMaster.cpp similarity index 67% rename from platforms/common/uORB/uORBDeviceMaster.cpp rename to src/systemcmds/uorb/uORBDeviceMaster.cpp index c39e4572db5e..92faf376fdb3 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/src/systemcmds/uorb/uORBDeviceMaster.cpp @@ -32,10 +32,11 @@ ****************************************************************************/ #include "uORBDeviceMaster.hpp" -#include "uORBDeviceNode.hpp" -#include "uORBManager.hpp" -#include "uORBUtils.hpp" +#include +#include +#include +#include #include #include @@ -45,6 +46,12 @@ #include #endif // PX4_QURT +#ifndef CONFIG_FS_SHMFS_VFS_PATH +#define CONFIG_FS_SHMFS_VFS_PATH "/dev/shm" +#endif + +#include + uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); @@ -55,115 +62,6 @@ uORB::DeviceMaster::~DeviceMaster() px4_sem_destroy(&_lock); } -int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance) -{ - int ret = PX4_ERROR; - - char nodepath[orb_maxpath]; - - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath(nodepath, meta, instance); - - if (ret != PX4_OK) { - return ret; - } - - ret = PX4_ERROR; - - /* try for topic groups */ - const unsigned max_group_tries = (instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - - if (instance) { - /* for an advertiser, this will be 0, but a for subscriber that requests a certain instance, - * we do not want to start with 0, but with the instance the subscriber actually requests. - */ - group_tries = *instance; - - if (group_tries >= max_group_tries) { - return -ENOMEM; - } - } - - SmartLock smart_lock(_lock); - - do { - /* if path is modifyable change try index */ - if (instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *instance = group_tries; - } - - /* construct the new node, passing the ownership of path to it */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, nodepath); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != PX4_OK) { - delete node; - - if (ret == -EEXIST) { - /* if the node exists already, get the existing one and check if it's advertised. */ - uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries); - - /* - * We can claim an existing node in these cases: - * - The node is not advertised (yet). It means there is already one or more subscribers or it was - * unadvertised. - * - We are a single-instance advertiser requesting the first instance. - * (Usually we don't end up here, but we might in case of a race condition between 2 - * advertisers). - * - We are a subscriber requesting a certain instance. - * (Also we usually don't end up in that case, but we might in case of a race condtion - * between an advertiser and subscriber). - */ - bool is_single_instance_advertiser = is_advertiser && !instance; - - if (existing_node != nullptr && - (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) { - if (is_advertiser) { - /* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers - * could get the same instance). - */ - existing_node->mark_as_advertised(); - } - - ret = PX4_OK; - - } else { - /* otherwise: already advertised, keep looking */ - } - } - - } else { - if (is_advertiser) { - node->mark_as_advertised(); - } - - // add to the node map. - _node_list.add(node); - _node_exists[node->get_instance()].set((uint8_t)node->id(), true); - } - - group_tries++; - - } while (ret != PX4_OK && (group_tries < max_group_tries)); - - if (ret != PX4_OK && group_tries >= max_group_tries) { - ret = -ENOMEM; - } - - return ret; -} - void uORB::DeviceMaster::printStatistics() { /* Add all nodes to a list while locked, and then print them in unlocked state, to avoid potential @@ -190,6 +88,7 @@ void uORB::DeviceMaster::printStatistics() DeviceNodeStatisticsData *prev = cur_node; cur_node = cur_node->next; + px4_munmap(prev->node, sizeof(uORB::DeviceNode)); delete prev; } } @@ -207,7 +106,36 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, } } - for (const auto &node : _node_list) { + DIR *shm_dir = opendir(CONFIG_FS_SHMFS_VFS_PATH); + struct dirent *shm; + const char orb_name_prefix[] = "_orb_"; + + while ((shm = readdir(shm_dir)) != nullptr) { + + if (strncmp(orb_name_prefix, shm->d_name, sizeof(orb_name_prefix) - 1)) { + continue; + } + + void *ptr = nullptr; + uORB::DeviceNode *node = nullptr; + + // open and mmap the shared memory segment + int shm_fd = shm_open(shm->d_name, O_RDWR, 0666); + + if (shm_fd >= 0) { + ptr = px4_mmap(0, sizeof(uORB::DeviceNode), PROT_READ, MAP_SHARED, shm_fd, 0); + } + + if (ptr != MAP_FAILED) { + node = static_cast(ptr); + } + + close(shm_fd); + + if (node == nullptr) { + PX4_ERR("Failed to MMAP an existing node\n"); + continue; + } ++num_topics; @@ -219,6 +147,8 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, } if (cur_node) { + // currently nuttx creates a new mapping on every mmap. TODO: check linux + px4_munmap(node, sizeof(uORB::DeviceNode)); continue; } @@ -260,6 +190,7 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, last_node->last_pub_msg_count = last_node->node->updates_available(0); } + closedir(shm_dir); return 0; } @@ -293,12 +224,6 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) lock(); - if (_node_list.empty()) { - unlock(); - PX4_INFO("no active topics"); - return; - } - DeviceNodeStatisticsData *first_node = nullptr; DeviceNodeStatisticsData *cur_node = nullptr; size_t max_topic_name_length = 0; @@ -424,36 +349,8 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) while (cur_node) { DeviceNodeStatisticsData *next_node = cur_node->next; - delete cur_node; + px4_munmap(cur_node->node, sizeof(uORB::DeviceNode)); + delete (cur_node); cur_node = next_node; } } - -#undef CLEAR_LINE - -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) -{ - lock(); - - for (uORB::DeviceNode *node : _node_list) { - if (strcmp(node->get_devname(), nodepath) == 0) { - unlock(); - return node; - } - } - - unlock(); - - return nullptr; -} - -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance) -{ - for (uORB::DeviceNode *node : _node_list) { - if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) { - return node; - } - } - - return nullptr; -} diff --git a/platforms/common/uORB/uORBDeviceMaster.hpp b/src/systemcmds/uorb/uORBDeviceMaster.hpp similarity index 70% rename from platforms/common/uORB/uORBDeviceMaster.hpp rename to src/systemcmds/uorb/uORBDeviceMaster.hpp index 94c97e61dfc9..f8f34c56ac8e 100644 --- a/platforms/common/uORB/uORBDeviceMaster.hpp +++ b/src/systemcmds/uorb/uORBDeviceMaster.hpp @@ -35,7 +35,7 @@ #include -#include "uORBCommon.hpp" +#include #include #include @@ -64,42 +64,8 @@ using px4::AtomicBitset; class uORB::DeviceMaster { public: - - int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance); - - /** - * Public interface for getDeviceNodeLocked(). Takes care of synchronization. - * @return node if exists, nullptr otherwise - */ - uORB::DeviceNode *getDeviceNode(const char *node_name); - uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) - { - if (meta == nullptr) { - return nullptr; - } - - if (!deviceNodeExists(static_cast(meta->o_id), instance)) { - return nullptr; - } - - lock(); - uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); - unlock(); - - //We can safely return the node that can be used by any thread, because - //a DeviceNode never gets deleted. - return node; - - } - - bool deviceNodeExists(ORB_ID id, const uint8_t instance) - { - if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) { - return false; - } - - return _node_exists[instance][(uint8_t)id]; - } + DeviceMaster(); + ~DeviceMaster(); /** * Print statistics for each existing topic. @@ -116,9 +82,6 @@ class uORB::DeviceMaster void showTop(char **topic_filter, int num_filters); private: - // Private constructor, uORB::Manager takes care of its creation - DeviceMaster(); - ~DeviceMaster(); struct DeviceNodeStatisticsData { DeviceNode *node; @@ -132,16 +95,6 @@ class uORB::DeviceMaster friend class uORB::Manager; - /** - * Find a node give its name. - * _lock must already be held when calling this. - * @return node if exists, nullptr otherwise - */ - uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance); - - IntrusiveSortedList _node_list; - AtomicBitset _node_exists[ORB_MULTI_MAX_INSTANCES]; - px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ void lock() { do {} while (px4_sem_wait(&_lock) != 0); } diff --git a/src/systemcmds/uorb/uorb.cpp b/src/systemcmds/uorb/uorb.cpp index b9fb565c143b..8a4561c28996 100644 --- a/src/systemcmds/uorb/uorb.cpp +++ b/src/systemcmds/uorb/uorb.cpp @@ -38,10 +38,28 @@ #include #include +#include "uORBDeviceMaster.hpp" + extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static void usage(); +static int uorb_status(void) +{ + uORB::DeviceMaster dev; + dev.printStatistics(); + + return OK; +} + +static int uorb_top(char **topic_filter, int num_filters) +{ + uORB::DeviceMaster dev; + dev.showTop(topic_filter, num_filters); + + return OK; +} + int uorb_main(int argc, char *argv[]) { if (argc < 2) { @@ -49,10 +67,7 @@ int uorb_main(int argc, char *argv[]) return -1; } - if (!strcmp(argv[1], "start")) { - return uorb_start(); - - } else if (!strcmp(argv[1], "status")) { + if (!strcmp(argv[1], "status")) { return uorb_status(); } else if (!strcmp(argv[1], "top")) { diff --git a/ssrc_config/README.txt b/ssrc_config/README.txt new file mode 100644 index 000000000000..0e1dc530af83 --- /dev/null +++ b/ssrc_config/README.txt @@ -0,0 +1,14 @@ +This folder contains extra parameter sets for different flying environments. +The config_.txt for each environment needs to be copied into sdcard as /etc/config.txt + +config_outdoor.txt +- Empty configuration + +config_indoor.txt +- Contains settings for maximum speeds (very slow), altitudes (very low), disabling GPS, magnetometer etc. + +config_hitl.txt +- Contains settings for configure px4 into hardware-in-the-loop mode and mavlink setup for UART gazebo connection + +config_hitl_eth.txt +- Contains same settings as config_hitl.txt, but the mavlink for gazebo connection is using ethernet diff --git a/ssrc_config/config_hitl.txt b/ssrc_config/config_hitl.txt new file mode 100644 index 000000000000..95a166843d3a --- /dev/null +++ b/ssrc_config/config_hitl.txt @@ -0,0 +1,59 @@ +# Default parameter set for HITL with UART gazebo connection (partly derived from indoor flying parameters) +# [ type: hitl ] + +##################################### +# HITL configuration +# + +# Set HITL flag +param set SYS_HITL 1 + +# HIL on TELEMETRY 2 +param set SER_TEL2_BAUD 2000000 +param set MAV_1_CONFIG 102 +param set MAV_1_MODE 2 +param set MAV_1_RATE 200000 + +# disable some checks to allow to fly +# - with usb +param set CBRK_USB_CHK 197848 +# - without real battery +param set CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC controller check +param set NAV_RCL_ACT 0 + + +##################################### +# Configs from indoor flight setup +# + +# Set waypoint acceptance radius to 0.5m +param set NAV_ACC_RAD 0.5 + +# Set height acceptance radius to 0.3m +param set NAV_MC_ALT_RAD 0.3 + +# Cruise speed +param set MPC_XY_CRUISE 0.5 + +# Track trajectory more aggressively (default 0.5) +param set MPC_XY_TRAJ_P 0.7 + +# Increase velocity controller P gain +param set MPC_XY_VEL_P_ACC 2.4 + +# Make it accelerate faster upwards at takeoff +param set MPC_TKO_RAMP_T 1.0 + +# Limit upward movement speed for safety +param set MPC_Z_VEL_MAX_UP 1.0 + +# Smoothing trajectory a bit when using AutoLineSmoothVel +param set MPC_JERK_AUTO 8 +param set MPC_ACC_HOR 3 + + diff --git a/ssrc_config/config_hitl_eth.txt b/ssrc_config/config_hitl_eth.txt new file mode 100644 index 000000000000..e443ebcc7d27 --- /dev/null +++ b/ssrc_config/config_hitl_eth.txt @@ -0,0 +1,24 @@ +# Default parameter set for HITL with ethernet gazebo connection (partly derived from indoor flying parameters) +# [ type: hitl_eth ] + +##################################### +# HITL configuration +# + +# Set HITL flag +param set SYS_HITL 1 + +# Start Mavlink for simulator connection +mavlink start -c 192.168.200.100 -u 14560 -o 14561 -m onboard -r 2000000 + +# disable some checks to allow to fly +# - with usb +param set CBRK_USB_CHK 197848 +# - without real battery +param set CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC controller check +param set NAV_RCL_ACT 0 diff --git a/ssrc_config/config_indoor.txt b/ssrc_config/config_indoor.txt new file mode 100644 index 000000000000..f78767b2cf99 --- /dev/null +++ b/ssrc_config/config_indoor.txt @@ -0,0 +1,60 @@ +# Default parameter set for indoor flying with mocap fake GPS +# [ type: indoor ] + +# Disable internal GPS +param set GPS_1_CONFIG 0 + +# Disable magnetometer +param set CAL_MAG0_PRIO 1 +param set CAL_MAG1_PRIO 0 +param set CAL_MAG2_PRIO 0 +param set CAL_MAG3_PRIO 0 +param set EKF2_MAG_TYPE 5 +param set SYS_HAS_MAG 0 + +# Set waypoint acceptance radius to 0.5m +param set NAV_ACC_RAD 0.5 + +# Set height acceptance radius to 0.3m +param set NAV_MC_ALT_RAD 0.3 + +# Enable LL40LS in i2c and use that as main height source for EKF +param set EKF2_HGT_REF 2 +param set SENS_EN_LL40LS 2 +param set EKF2_RNG_CTRL 2 + +# Enable GPS Yaw fusion +param set EKF2_GPS_CTRL 11 + +# Disable external vision aid +param set EKF2_EV_CTRL 0 + +# RTL altitudes to avoid hitting the roof +param set RTL_DESCEND_ALT 2 +param set RTL_RETURN_ALT 2 + +# Offboard failsafe, set to land immediately even when RC is available +param set COM_OBL_RC_ACT 4 + +# RC failsafe, land immediately +param set NAV_RCL_ACT 3 + +# Cruise speed +param set MPC_XY_CRUISE 2.0 + +# Track trajectory more aggressively (default 0.5) +param set MPC_XY_TRAJ_P 0.7 + +# Limit upward movement speed for safety +param set MPC_Z_VEL_MAX_UP 1.0 + +# Smoothing trajectory a bit when using AutoLineSmoothVel +param set MPC_JERK_AUTO 8 +param set MPC_ACC_HOR 3 + +# Default Flight Modes and Kill Switch +param set COM_FLTMODE1 0 +param set COM_FLTMODE4 2 +param set COM_FLTMODE6 11 +param set RC_MAP_FLTMODE 5 +param set RC_MAP_KILL_SW 7 diff --git a/ssrc_config/config_indoor_sim.txt b/ssrc_config/config_indoor_sim.txt new file mode 100644 index 000000000000..4b0777c923e3 --- /dev/null +++ b/ssrc_config/config_indoor_sim.txt @@ -0,0 +1,44 @@ +# Default parameter set for indoor flying in simulation (with regular gps simulation) +# [ type: indoor_sim ] + +# Set waypoint acceptance radius to 0.5m +param set NAV_ACC_RAD 0.5 + +# Set height acceptance radius to 0.3m +param set NAV_MC_ALT_RAD 0.3 + +# RTL altitudes to avoid hitting the roof +param set RTL_DESCEND_ALT 2 +param set RTL_RETURN_ALT 2 + +# Offboard failsafe, set to land immediately even when RC is available +param set COM_OBL_RC_ACT 4 + +# RC failsafe, land immediately +param set NAV_RCL_ACT 3 + +# Cruise speed +param set MPC_XY_CRUISE 2.0 + +# Track trajectory more aggressively (default 0.5) +param set MPC_XY_TRAJ_P 0.7 + +# Increase velocity controller P gain +param set MPC_XY_VEL_P_ACC 1.9 + +# Make it accelerate faster upwards at takeoff +param set MPC_TKO_RAMP_T 1.0 + +# Limit upward movement speed for safety +param set MPC_Z_VEL_MAX_UP 1.0 + +# Smoothing trajectory a bit when using AutoLineSmoothVel +param set MPC_JERK_AUTO 8 +param set MPC_ACC_HOR 3 + +# Default Flight Modes and Kill Switch +param set COM_FLTMODE1 0 +param set COM_FLTMODE4 2 +param set COM_FLTMODE6 11 +param set RC_MAP_FLTMODE 5 +param set RC_MAP_KILL_SW 7 diff --git a/ssrc_config/config_outdoor.txt b/ssrc_config/config_outdoor.txt new file mode 100644 index 000000000000..16f468f50167 --- /dev/null +++ b/ssrc_config/config_outdoor.txt @@ -0,0 +1,12 @@ +# Default parameter set for indoor flying with lighthouse tracker / libsurvive fake GPS +# [ type: outdoor ] + +# This file should contain only testing/safety related parameters +# and nothing that affects autonomous flight + +# Default Flight Modes and Kill Switch +param set COM_FLTMODE1 0 +param set COM_FLTMODE4 2 +param set COM_FLTMODE6 11 +param set RC_MAP_FLTMODE 5 +param set RC_MAP_KILL_SW 7 diff --git a/tasks.py b/tasks.py new file mode 100644 index 000000000000..5e800bd1bc8d --- /dev/null +++ b/tasks.py @@ -0,0 +1,121 @@ +import glob +import os +from invoke import task, Collection, call + +THISDIR = os.path.dirname(os.path.realpath(__file__)) +MODULE_NAME = os.path.basename(THISDIR) + +def get_submodules(c): + """ + return repository submodule names + """ + submodules = [] + with c.cd(THISDIR): + result = c.run("git submodule status", hide=True) + for line in result.stdout.splitlines(): + submodules.append(line.split()[1]) + return submodules + +def get_iname_tag(image_name): + """ + return tuple with image name and tag + """ + if ":" in image_name: + iname, tag = image_name.split(":") + else: + iname, tag = image_name, "latest" + return iname, tag + + +@task +def init(c): + """ + Init submodules. + """ + print("init submodules") + with c.cd(THISDIR): + c.run("git submodule init", hide=True) + +@task(init) +def clone(c): + """ + Clone this repository submodules. + """ + submodules = get_submodules(c) + with c.cd(THISDIR): + for sub in submodules: + c.run("git submodule update --init --recursive %s" %sub) + +@task( + help={'nocache': "do not use cache when building the image", + 'pull': "always attempt to pull a newer version of the image", + 'ros_distro': "ROS distro to use (Available [humble])"} +) +def build_sitl(c, nocache=False, pull=False, ros_distro="humble", image_name=MODULE_NAME): + """ + Create Docker build environment. + """ + iname, tag = get_iname_tag(image_name) + + args = [] + args.append("--build-arg VERSION=$(git describe --always --tags --dirty | sed 's/^v//')") + args.append("-f packaging/Dockerfile.sitl") + args.append("-t %s_build:%s" % (iname, tag)) + if nocache: + args.append("--no-cache") + elif pull: + args.append("--pull") + with c.cd(THISDIR): + c.run("docker build %s ." % " ".join(args)) + +@task( + help={'reallyclean': "remove & reload all submodules"} +) +def clean(c, reallyclean=False): + """ + Clean workspace. + """ + with c.cd(THISDIR): + if reallyclean: + c.run("git submodule deinit -f --all") + clone(c) + else: + c.run("git submodule foreach git clean -xdf") + c.run("git submodule foreach git checkout .") + c.run("git clean -xdf") + +@task( + help={'out_dir': "output directory for the generated deb files", + 'ros_distro': "ROS distro to use (Available [humble])"} +) +def create_deb_package(c, out_dir="../bin/", ros_distro="humble", image_name=MODULE_NAME): + """ + Build debian package + """ + c.run("./build.sh {0}" + .format(out_dir)) + +@task(help={'nocache': "do not use cache when building the image", + 'pull': "always attempt to pull a newer version of the image", + 'image_name': "name of output docker image"} +) +def build_docker(c, nocache=False, pull=False, image_name=MODULE_NAME): + """ + Build Docker image of this component + """ + col = Collection() + col.add_task(build_sitl) + col['build_sitl'](c, nocache=nocache, pull=pull, image_name=image_name) + + iname, tag = get_iname_tag(image_name) + args = [] + args.append("--build-arg FROM_ARTIFACTS=%s_build:%s" % (iname, tag)) + args.append("-f packaging/Dockerfile.docker") + args.append("-t %s:%s" % (iname, tag)) + if nocache: + args.append("--no-cache") + elif pull: + args.append("--pull") + with c.cd(THISDIR): + print("docker build %s ." % " ".join(args)) + c.run("docker build %s ." % " ".join(args)) diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 90b27f943aac..279365f930d1 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -24,9 +24,9 @@ if(MAVSDK_FOUND) test_multicopter_failure_injection.cpp test_multicopter_failsafe.cpp test_multicopter_mission.cpp - test_multicopter_offboard.cpp - test_multicopter_manual.cpp - test_vtol_mission.cpp + # test_multicopter_offboard.cpp + # test_multicopter_manual.cpp + # test_vtol_mission.cpp # test_multicopter_follow_me.cpp ) diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index daff0aefc6c9..60ba8fc49075 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -7,7 +7,7 @@ { "model": "iris", "vehicle": "iris", - "test_filter": "[multicopter],[offboard]", + "test_filter": "[multicopter]", "timeout_min": 10 }, {