fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 00:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/PX4/Firmware

Browse Source
This commit is contained in:
Thomas Gubler
2012-08-19 14:14:55 +02:00
parent 1cf92ad605 2a5fcd9174
commit b440b687aa
63 changed files with 790 additions and 6650 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+1 -1
View File
@@ -131,7 +131,7 @@ endif
#
.PHONY: clean
clean:
@make -C $(NUTTX_SRC) -r $(MQUIET) clean
@make -C $(NUTTX_SRC) -r $(MQUIET) distclean
@make -C $(ROMFS_SRC) -r $(MQUIET) clean
.PHONY: distclean
ROMFS/scripts/rc.PX4IOAR
+4 -3
View File
@@ -19,6 +19,7 @@ sh /etc/init.d/rc.sensors
# Start MAVLink
#
mavlink -d /dev/ttyS0 -b 57600 &
usleep 5000
#
# Start the commander.
@@ -40,7 +41,7 @@ attitude_estimator_bm &
#
# XXX arguments?
#
px4fmu start
#fmu mode_
#
# Fire up the AR.Drone controller.
@@ -54,12 +55,12 @@ ardrone_control -d /dev/ttyS1 -m attitude &
#
# XXX this should not need to be backgrounded
#
gps -d /dev/ttyS3 -m all &
#gps -d /dev/ttyS3 -m all &
#
# Start logging to microSD if we can
#
sh /etc/init.d/rc.logging
#sh /etc/init.d/rc.logging
#
# startup is done; we don't want the shell because we
apps/ardrone_control/Makefile
+1 -1
View File
@@ -37,7 +37,7 @@
APPNAME = ardrone_control
PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 3096
STACKSIZE = 4096
# explicit list of sources - not everything is built currently
CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
apps/ardrone_control/ardrone_control.c
+1 -1
View File
@@ -236,7 +236,7 @@ int ardrone_control_main(int argc, char *argv[])
state.state_machine == SYSTEM_STATE_AUTO ||
state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
att_sp.thrust = manual.throttle/2.0f;
att_sp.thrust = manual.throttle;
} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */
apps/commander/Makefile
+2 -2
View File
@@ -36,8 +36,8 @@
#
APPNAME = commander
PRIORITY = SCHED_PRIORITY_MAX - 10
STACKSIZE = 3072
PRIORITY = SCHED_PRIORITY_MAX - 30
STACKSIZE = 4096
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
apps/commander/commander.c
+49 -33
View File
@@ -95,10 +95,7 @@ static int leds;
static int buzzer;
static int mavlink_fd;
static bool commander_initialized = false;
static struct vehicle_status_s current_status = {
.state_machine = SYSTEM_STATE_PREFLIGHT,
.mode = 0
}; /**< Main state machine */
static struct vehicle_status_s current_status; /**< Main state machine */
static int stat_pub;
static uint16_t nofix_counter = 0;
@@ -306,8 +303,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
calibration_counter++;
} else {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
break;
//mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
//break;
}
}
@@ -376,9 +373,22 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
printf("\nFINAL:\nmag min: %d\t%d\t%d\nmag max: %d\t%d\t%d\n", (int)min_avg[0], (int)min_avg[1], (int)min_avg[2], (int)max_avg[0], (int)max_avg[1], (int)max_avg[2]);
float mag_offset[3];
mag_offset[0] = (max_avg[0] - min_avg[0])/2;
mag_offset[1] = (max_avg[1] - min_avg[1])/2;
mag_offset[2] = (max_avg[2] - min_avg[2])/2;
/**
* The offset is subtracted from the sensor values, so the result is the
* POSITIVE number that has to be subtracted from the sensor data
* to shift the center to zero
*
* offset = max - ((max - min) / 2.0f)
*
* which reduces to
*
* offset = (max + min) / 2.0f
*/
mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f;
mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f;
mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f;
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET] = mag_offset[0];
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET] = mag_offset[1];
@@ -532,8 +542,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration");
do_gyro_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
@@ -549,12 +560,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration");
do_mag_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration");
result = MAV_RESULT_DENIED;
}
handled = true;
@@ -562,8 +574,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* none found */
if (!handled) {
fprintf(stderr, "[commander] refusing unsupported calibration request\n");
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request");
//fprintf(stderr, "[commander] refusing unsupported calibration request\n");
mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request");
result = MAV_RESULT_UNSUPPORTED;
}
}
@@ -576,12 +588,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if (((int)cmd->param1) == 0) {
if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
printf("[commander] Loaded EEPROM params in RAM\n");
mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM");
//printf("[commander] Loaded EEPROM params in RAM\n");
mavlink_log_info(mavlink_fd, "[commander] CMD Loaded EEPROM params in RAM");
result = MAV_RESULT_ACCEPTED;
} else {
fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
//fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
result = MAV_RESULT_FAILED;
}
@@ -591,18 +603,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} else if (((int)cmd->param1) == 1) {
if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
printf("[commander] RAM params written to EEPROM\n");
//printf("[commander] RAM params written to EEPROM\n");
mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM");
result = MAV_RESULT_ACCEPTED;
} else {
fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
//fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM");
result = MAV_RESULT_FAILED;
}
} else {
fprintf(stderr, "[commander] refusing unsupported storage request\n");
//fprintf(stderr, "[commander] refusing unsupported storage request\n");
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request");
result = MAV_RESULT_UNSUPPORTED;
}
@@ -622,8 +634,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* send any requested ACKs */
if (cmd->confirmation > 0) {
/* send acknowledge command */
mavlink_message_t msg;
mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
}
}
@@ -785,9 +795,11 @@ int commander_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
current_status.flag_system_armed = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
/* publish current state machine */
state_machine_publish(stat_pub, &current_status, mavlink_fd);
if (stat_pub < 0) {
@@ -874,9 +886,9 @@ int commander_main(int argc, char *argv[])
battery_voltage_valid = sensors.battery_voltage_valid;
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
// flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
/* Slow but important 5 Hz checks */
/* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
@@ -891,8 +903,8 @@ int commander_main(int argc, char *argv[])
}
/* toggle error led at 5 Hz in HIL mode */
if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* armed */
if (current_status.flag_hil_enabled) {
/* hil enabled */
led_toggle(LED_AMBER);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
@@ -994,14 +1006,14 @@ int commander_main(int argc, char *argv[])
//
//
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */
/* Check battery voltage */
/* write to sys_status */
current_status.voltage_battery = battery_voltage;
/* if battery voltage is getting lower, warn using buzzer, etc. */
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
@@ -1086,8 +1098,8 @@ int commander_main(int argc, char *argv[])
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 2 s / 2000 ms) */
if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 3000000)) {
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
last_print_time = hrt_absolute_time();
}
@@ -1105,18 +1117,22 @@ int commander_main(int argc, char *argv[])
current_status.counter++;
current_status.timestamp = hrt_absolute_time();
if (voltage_previous != current_status.voltage_battery) orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
/* If full run came back clean, transition to standby */
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
current_status.preflight_gyro_calibration == false &&
current_status.preflight_mag_calibration == false) {
/* All ok, no calibration going on, go to standby */
do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY, mavlink_fd);
do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
}
/* publish at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
}
/* Store old modes to detect and act on state transitions */
// vehicle_state_previous = current_status.state_machine;
voltage_previous = current_status.voltage_battery;
fflush(stdout);
apps/commander/state_machine_helper.c
+61 -36
View File
@@ -83,14 +83,14 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
} else {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
}
return;
}
break;
case SYSTEM_STATE_EMCY_LANDING:
/* Tell the controller to land */
//TODO: add emcy landing code here
/* set system flags according to state */
current_status->flag_system_armed = true;
fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
@@ -98,11 +98,21 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
case SYSTEM_STATE_EMCY_CUTOFF:
/* Tell the controller to cutoff the motors (thrust = 0) */
/* set system flags according to state */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
break;
case SYSTEM_STATE_GROUND_ERROR:
/* set system flags according to state */
/* prevent actuators from arming */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
break;
@@ -110,8 +120,9 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
case SYSTEM_STATE_PREFLIGHT:
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
mavlink_log_info(mavlink_fd, "[commander] Switched to PREFLIGHT state");
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
@@ -122,6 +133,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
invalid_state = false;
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
usleep(500000);
reboot();
@@ -133,22 +146,47 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
break;
case SYSTEM_STATE_STANDBY:
/* set system flags according to state */
/* standby enforces disarmed */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
break;
case SYSTEM_STATE_GROUND_READY:
/* set system flags according to state */
/* ground ready has motors / actuators armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
break;
case SYSTEM_STATE_AUTO:
/* set system flags according to state */
/* auto is airborne and in auto mode, motors armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
break;
case SYSTEM_STATE_STABILIZED:
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
break;
case SYSTEM_STATE_MANUAL:
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
break;
@@ -161,6 +199,9 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
}
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
@@ -421,15 +462,10 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
// XXX CHANGE BACK
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
printf("[commander] arming\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
} /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] landing\n");
do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
}*/
}
}
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
@@ -448,7 +484,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->control_manual_enabled = true;
current_status->flag_control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -461,7 +497,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->control_manual_enabled = true;
current_status->flag_control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -474,9 +510,9 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->control_manual_enabled = true;
current_status->flag_control_manual_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
@@ -489,20 +525,11 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
printf("in update state request\n");
uint8_t ret = 1;
current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */
current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
/* vehicle is disarmed, mode requests arming */
if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
/* only arm in standby state */
// XXX REMOVE
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* Set armed flag */
current_status->mode |= VEHICLE_MODE_FLAG_SAFETY_ARMED;
/* Set manual input enabled flag */
current_status->mode |= VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK;
printf("[commander] arming due to command request\n");
@@ -510,28 +537,26 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
}
/* vehicle is armed, mode requests disarming */
//if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
/* only disarm in ground ready */
//if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
/* Clear armed flag, leave manual input enabled */
// current_status->mode &= ~VEHICLE_MODE_FLAG_SAFETY_ARMED;
// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[commander] disarming due to command request\n");
//}
//}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
ret = OK;
printf("[commander] disarming due to command request\n");
}
}
/* Switch on HIL if in standby */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* Enable HIL on request */
current_status->mode |= VEHICLE_MODE_FLAG_HIL_ENABLED;
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
printf("[commander] Enabling HIL\n");
}
/* NEVER actually switch off HIL without reboot */
if ((current_status->mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
ret = ERROR;
}
apps/px4/sensors_bringup/Makefile → apps/examples/px4_simple_app/Makefile
+2 -2
View File
@@ -32,10 +32,10 @@
############################################################################
#
# Makefile to build the sensor bringup tests
# Basic example application
#
APPNAME = sensors_bringup
APPNAME = px4_simple_app
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
apps/examples/px4_simple_app/px4_simple_app.c
+115
View File
@@ -0,0 +1,115 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* 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 px4_simple_app.c
* Minimal application example for PX4 autopilot
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
int px4_simple_app_main(int argc, char *argv[])
{
printf("Hello Sky!\n");
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
orb_set_interval(sensor_sub_fd, 1000);
/* advertise attitude topic */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);
/* one could wait for multiple topics with this technique, just using one here */
struct pollfd fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
/* there could be more file descriptors here, in the form like:
* { .fd = other_sub_fd, .events = POLLIN },
*/
};
int error_counter = 0;
while (true) {
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
printf("[px4_simple_app] Got no data within a second\n");
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
printf("[px4_simple_app] ERROR return value from poll(): %d\n"
, poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
return 0;
}
apps/mavlink/mavlink.c
+139 -54
View File
@@ -78,6 +78,11 @@
__EXPORT int mavlink_main(int argc, char *argv[]);
int mavlink_thread_main(int argc, char *argv[]);
static bool thread_should_exit = false;
static bool thread_running = false;
static int mavlink_task;
/* terminate MAVLink on user request - disabled by default */
static bool mavlink_link_termination_allowed = false;
@@ -92,7 +97,6 @@ static mavlink_status_t status;
static pthread_t receive_thread;
static pthread_t uorb_receive_thread;
static uint16_t mavlink_message_intervals[256]; /**< intervals at which to send MAVLink packets */
/* Allocate storage space for waypoints */
mavlink_wpm_storage wpm_s;
@@ -125,6 +129,7 @@ static struct vehicle_command_s vcmd;
static int pub_hil_global_pos = -1;
static int ardrone_motors_pub = -1;
static int cmd_pub = -1;
static int sensor_sub = -1;
static int global_pos_sub = -1;
static int local_pos_sub = -1;
static int flow_pub = -1;
@@ -154,7 +159,7 @@ void handleMessage(mavlink_message_t *msg);
/**
* Enable / disable Hardware in the Loop simulation mode.
*/
int set_hil_on_off(uint8_t vehicle_mode);
int set_hil_on_off(bool hil_enabled);
/**
* Translate the custom state into standard mavlink modes and state.
@@ -170,6 +175,11 @@ mavlink_wpm_storage *wpm;
#include "mavlink_parameters.h"
/**
* Print the usage
*/
static void usage(const char *reason);
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
void mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -281,12 +291,12 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa
int set_hil_on_off(uint8_t vehicle_mode)
int set_hil_on_off(bool hil_enabled)
{
int ret = OK;
/* Enable HIL */
if ((vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && !mavlink_hil_enabled) {
if (hil_enabled && !mavlink_hil_enabled) {
//printf("\n HIL ON \n");
@@ -308,7 +318,7 @@ int set_hil_on_off(uint8_t vehicle_mode)
}
}
if (!(vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && mavlink_hil_enabled) {
if (!hil_enabled && mavlink_hil_enabled) {
mavlink_hil_enabled = false;
(void)close(pub_hil_attitude);
(void)close(pub_hil_global_pos);
@@ -326,7 +336,7 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t
*mavlink_mode = 0;
/* set mode flags independent of system state */
if (c_status->control_manual_enabled) {
if (c_status->flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
@@ -425,7 +435,6 @@ static void *receiveloop(void *arg)
/* Handle packet with parameter component */
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
msg.msgid = -1;
}
}
@@ -433,6 +442,28 @@ static void *receiveloop(void *arg)
return NULL;
}
static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
{
int ret = OK;
switch (mavlink_msg_id) {
case MAVLINK_MSG_ID_SCALED_IMU:
/* senser sub triggers scaled IMU */
orb_set_interval(sensor_sub, min_interval);
break;
case MAVLINK_MSG_ID_RAW_IMU:
/* senser sub triggers RAW IMU */
orb_set_interval(sensor_sub, min_interval);
break;
default:
/* not found */
ret = ERROR;
break;
}
return ret;
}
/**
* Listen for uORB topics and send via MAVLink.
*
@@ -467,7 +498,7 @@ static void *uorb_receiveloop(void *arg)
/* --- SENSORS RAW VALUE --- */
/* subscribe to ORB for sensors raw */
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
orb_set_interval(sensor_sub, 100); /* 10Hz updates */
fds[fdsc_count].fd = sensor_sub;
fds[fdsc_count].events = POLLIN;
@@ -604,8 +635,8 @@ static void *uorb_receiveloop(void *arg)
/* send raw imu data */
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
/* send scaled imu data */
mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 9810, buf.raw.accelerometer_m_s2[1] * 9810, buf.raw.accelerometer_m_s2[2] * 9810, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
/* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */
mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
/* send pressure */
mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100);
@@ -663,11 +694,11 @@ static void *uorb_receiveloop(void *arg)
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
/* enable or disable HIL */
set_hil_on_off(v_status.mode);
set_hil_on_off(v_status.flag_hil_enabled);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = v_status.mode;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
/* send heartbeat */
@@ -692,7 +723,7 @@ static void *uorb_receiveloop(void *arg)
fw_control.attitude_control_output[3]);
/* Only send in HIL mode */
if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) {
if (v_status.flag_hil_enabled) {
/* Send the desired attitude from RC or from the autonomous controller */
// XXX it should not depend on a RC setting, but on a system_state value
@@ -899,10 +930,11 @@ void handleMessage(mavlink_message_t *msg)
/* check if topic is advertised */
if (flow_pub <= 0) {
flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
/* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
}
/* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &flow);
}
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
@@ -925,6 +957,10 @@ void handleMessage(mavlink_message_t *msg)
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
}
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
@@ -963,7 +999,6 @@ void handleMessage(mavlink_message_t *msg)
*/
// printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false");
#define DEG2RAD ((1.0/180.0)*M_PI)
if (mavlink_hil_enabled) {
@@ -1169,7 +1204,7 @@ int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_
/**
* MAVLink Protocol main function.
*/
int mavlink_main(int argc, char *argv[])
int mavlink_thread_main(int argc, char *argv[])
{
wpm = &wpm_s;
@@ -1190,54 +1225,41 @@ int mavlink_main(int argc, char *argv[])
/* reate the device node that's used for sending text log messages, etc. */
register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
/* Send attitude at 10 Hz / every 100 ms */
mavlink_message_intervals[MAVLINK_MSG_ID_ATTITUDE] = 100;
/* Send raw sensor values at 10 Hz / every 100 ms */
mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100;
/* default values for arguments */
char *uart_name = "/dev/ttyS0";
int baudrate = 57600;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n";
/* read program arguments */
int i;
for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
}
/* UART device ID */
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
usage("");
return 0;
} else if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
uart_name = argv[i + 1];
i++;
} else {
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
usage("missing argument for device (-d)");
return 1;
}
}
/* baud rate */
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
} else if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
if (argc > i + 1) {
baudrate = atoi(argv[i + 1]);
i++;
} else {
printf(commandline_usage, argv[0], uart_name, baudrate);
return 0;
usage("missing argument for baud rate (-b)");
return 1;
}
}
/* terminating MAVLink is allowed - yes/no */
if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
} else if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) {
mavlink_link_termination_allowed = true;
}
if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
} else if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) {
mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
} else {
usage("out of order or invalid argument");
return 1;
}
}
@@ -1271,8 +1293,8 @@ int mavlink_main(int argc, char *argv[])
pthread_attr_t uorb_attr;
pthread_attr_init(&uorb_attr);
/* Set stack size, needs more than 2048 bytes */
pthread_attr_setstacksize(&uorb_attr, 5096);
/* Set stack size, needs more than 4000 bytes */
pthread_attr_setstacksize(&uorb_attr, 4096);
pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
/* initialize waypoint manager */
@@ -1281,9 +1303,23 @@ int mavlink_main(int argc, char *argv[])
uint16_t counter = 0;
int lowspeed_counter = 0;
/**< Subscribe to system state and RC channels */
// int status_sub = orb_subscribe(ORB_ID(vehicle_status));
// int rc_sub = orb_subscribe(ORB_ID(rc_channels));
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 921600) {
/* 1000 Hz / 1 ms */
set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1);
} else if (baudrate >= 460800) {
/* 250 Hz / 4 ms */
set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000);
}
while (1) {
@@ -1303,7 +1339,7 @@ int mavlink_main(int argc, char *argv[])
if (lowspeed_counter == 10) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = v_status.mode;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode);
/* send heartbeat */
@@ -1316,8 +1352,10 @@ int mavlink_main(int argc, char *argv[])
v_status.errors_count1, v_status.errors_count2, v_status.errors_count3, v_status.errors_count4);
/* send over MAVLink */
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
if ((hrt_absolute_time() - rc.timestamp) < 200000) {
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi);
}
lowspeed_counter = 0;
}
@@ -1356,7 +1394,54 @@ int mavlink_main(int argc, char *argv[])
fflush(stdout);
fflush(stderr);
thread_running = false;
return 0;
}
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: mavlink {start|stop|status} [-d <devicename>] [-b <baudrate>] [-e/--exit-allowed]\n\n");
exit(1);
}
int mavlink_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("mavlink already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4096, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmavlink app is running\n");
} else {
printf("\tmavlink app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
apps/px4/attitude_estimator_bm/Makefile
+1 -1
View File
@@ -37,6 +37,6 @@
APPNAME = attitude_estimator_bm
PRIORITY = SCHED_PRIORITY_MAX - 10
STACKSIZE = 4096
STACKSIZE = 12000
include $(APPDIR)/mk/app.mk
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+14 -10
View File
@@ -87,14 +87,14 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
gyro_values.z = raw->gyro_rad_s[2];
float_vect3 accel_values;
accel_values.x = raw->accelerometer_m_s2[0];
accel_values.y = raw->accelerometer_m_s2[1];
accel_values.z = raw->accelerometer_m_s2[2];
accel_values.x = raw->accelerometer_m_s2[0] * 9.81f;
accel_values.y = raw->accelerometer_m_s2[1] * 9.81f;
accel_values.z = raw->accelerometer_m_s2[2] * 9.81f;
float_vect3 mag_values;
mag_values.x = raw->magnetometer_ga[0];
mag_values.y = raw->magnetometer_ga[1];
mag_values.z = raw->magnetometer_ga[2];
mag_values.x = raw->magnetometer_ga[0]*100.0f;
mag_values.y = raw->magnetometer_ga[1]*100.0f;
mag_values.z = raw->magnetometer_ga[2]*100.0f;
attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
@@ -215,10 +215,14 @@ int attitude_estimator_bm_main(int argc, char *argv[])
att.timestamp = sensor_combined_s_local.timestamp;
att.roll = euler.x;
att.pitch = euler.y;
att.yaw = euler.z + M_PI;
att.yaw = euler.z - M_PI_F;
if (att.yaw > 2.0f * ((float)M_PI)) {
att.yaw -= 2.0f * ((float)M_PI);
if (att.yaw > M_PI_F) {
att.yaw -= 2.0f * M_PI_F;
}
if (att.yaw < -M_PI_F) {
att.yaw += 2.0f * M_PI_F;
}
att.rollspeed = rates.x;
@@ -241,7 +245,7 @@ int attitude_estimator_bm_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* switching from non-HIL to HIL mode */
//printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
if (vstatus.flag_hil_enabled && !hil_enabled) {
hil_enabled = true;
publishing = false;
int ret = close(pub_att);
apps/px4/px4io/driver/px4io.cpp
+1 -1
View File
@@ -65,7 +65,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/hx_stream.h>
#include "../protocol.h"
#include "px4io/protocol.h"
#include "uploader.h"
class PX4IO;
apps/px4/sensors_bringup/.context
View File
apps/px4/sensors_bringup/bma180.c
-209
View File
@@ -1,209 +0,0 @@
/*
* Operations for the Bosch BMA180 3D Accelerometer
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <arch/board/board.h>
#include <nuttx/spi.h>
#include "sensors.h"
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define ADDR_CHIP_ID 0x00
#define CHIP_ID 0x03
#define ADDR_VERSION 0x01
#define ADDR_CTRL_REG0 0x0D
#define ADDR_CTRL_REG1 0x0E
#define ADDR_CTRL_REG2 0x0F
#define ADDR_BWTCS 0x20
#define ADDR_CTRL_REG3 0x21
#define ADDR_CTRL_REG4 0x22
#define ADDR_OLSB1 0x35
#define ADDR_ACC_X_LSB 0x02
#define ADDR_ACC_Z_MSB 0x07
#define ADDR_TEMPERATURE 0x08
#define ADDR_STATUS_REG1 0x09
#define ADDR_STATUS_REG2 0x0A
#define ADDR_STATUS_REG3 0x0B
#define ADDR_STATUS_REG4 0x0C
#define ADDR_RESET 0x10
#define SOFT_RESET 0xB6
#define ADDR_DIS_I2C 0x27
#define REG0_WRITE_ENABLE 0x10
#define BWTCS_LP_10HZ (0<<4)
#define BWTCS_LP_20HZ (1<<4)
#define BWTCS_LP_40HZ (2<<4)
#define BWTCS_LP_75HZ (3<<4)
#define BWTCS_LP_150HZ (4<<4)
#define BWTCS_LP_300HZ (5<<4)
#define BWTCS_LP_600HZ (6<<4)
#define BWTCS_LP_1200HZ (7<<4)
#define RANGE_1G (0<<1)
#define RANGE_1_5G (1<<1)
#define RANGE_2G (2<<1)
#define RANGE_3G (3<<1)
#define RANGE_4G (4<<1)
#define RANGE_8G (5<<1)
#define RANGE_16G (6<<1)
#define RANGEMASK 0x0E
#define BWMASK 0xF0
static void
write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true);
SPI_SNDBLOCK(spi, &cmd, sizeof(cmd));
SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false);
}
static uint8_t
read_reg(struct spi_dev_s *spi, uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true);
SPI_EXCHANGE(spi, cmd, data, sizeof(cmd));
SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false);
return data[1];
}
int
bma180_test_configure(struct spi_dev_s *spi)
{
uint8_t id;
id = read_reg(spi, ADDR_CHIP_ID);
uint8_t version = read_reg(spi, 0x01);
if (id == CHIP_ID)
{
message("BMA180 SUCCESS: 0x%02x, version: %d\n", id, version);
}
else
{
message("BMA180 FAIL: 0x%02x\n", id);
}
//message("got id 0x%02x, expected ID 0x03\n", id);
write_reg(spi, ADDR_RESET, SOFT_RESET); // page 48
usleep(12000); // wait 10 ms, see page 49
// Configuring the BMA180
/* enable writing to chip config */
uint8_t ctrl0 = read_reg(spi, ADDR_CTRL_REG0);
ctrl0 |= REG0_WRITE_ENABLE;
write_reg(spi, ADDR_CTRL_REG0, ctrl0);
uint8_t disi2c = read_reg(spi, ADDR_DIS_I2C); // read
disi2c |= 0x01; // set bit0 to 1, SPI only
write_reg(spi, ADDR_DIS_I2C, disi2c); // Set spi, disable i2c, page 31
/* set bandwidth */
uint8_t bwtcs = read_reg(spi, ADDR_BWTCS);
printf("bwtcs: %d\n", bwtcs);
bwtcs &= (~BWMASK);
bwtcs |= (BWTCS_LP_600HZ);// & BWMASK);
write_reg(spi, ADDR_BWTCS, bwtcs);
/* set range */
uint8_t olsb1 = read_reg(spi, ADDR_OLSB1);
printf("olsb1: %d\n", olsb1);
olsb1 &= (~RANGEMASK);
olsb1 |= (RANGE_4G);// & RANGEMASK);
write_reg(spi, ADDR_OLSB1, olsb1);
// uint8_t reg3 = read_reg(spi, ADDR_CTRL_REG3);
// //reg3 &= 0xFD; // REset bit 1 enable interrupt
// //reg3 |= 0x02; // enable
// write_reg(spi, ADDR_CTRL_REG3, reg3); //
/* block writing to chip config */
ctrl0 = read_reg(spi, ADDR_CTRL_REG0);
ctrl0 &= (~REG0_WRITE_ENABLE);
printf("ctrl0: %d\n", ctrl0);
write_reg(spi, ADDR_CTRL_REG0, ctrl0);
return 0;
}
int
bma180_test_read(struct spi_dev_s *spi)
{
struct { /* status register and data as read back from the device */
uint8_t cmd;
int16_t x;
int16_t y;
int16_t z;
uint8_t temp;
} __attribute__((packed)) report;
report.x = 0;
report.y = 0;
report.z = 0;
// uint8_t temp;
// uint8_t status1;
// uint8_t status2;
// uint8_t status3;
// uint8_t status4;
report.cmd = ADDR_ACC_X_LSB | DIR_READ | ADDR_INCREMENT;
//SPI_LOCK(spi, true);
//SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true);
//SPI_EXCHANGE(spi, &report, &report, sizeof(report));
//SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false);
//SPI_LOCK(spi, false);
report.x = read_reg(spi, ADDR_ACC_X_LSB);
report.x |= (read_reg(spi, ADDR_ACC_X_LSB+1) << 8);
report.y = read_reg(spi, ADDR_ACC_X_LSB+2);
report.y |= (read_reg(spi, ADDR_ACC_X_LSB+3) << 8);
report.z = read_reg(spi, ADDR_ACC_X_LSB+4);
report.z |= (read_reg(spi, ADDR_ACC_X_LSB+5) << 8);
report.temp = read_reg(spi, ADDR_ACC_X_LSB+6);
// Collect status and remove two top bits
uint8_t new_data = (report.x & 0x01) + (report.x & 0x01) + (report.x & 0x01);
report.x = (report.x >> 2);
report.y = (report.y >> 2);
report.z = (report.z >> 2);
message("ACC: x: %d\ty: %d\tz: %d\ttemp: %d new: %d\n", report.x, report.y, report.z, report.temp, new_data);
usleep(2000);
return 0;
}
apps/px4/sensors_bringup/l3gd20.c
-184
View File
@@ -1,184 +0,0 @@
/*
* Operations for the l3g4200
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <arch/board/board.h>
#include <nuttx/spi.h>
#include "sensors.h"
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define ADDR_WHO_AM_I 0x0f
#define WHO_I_AM 0xd4
#define ADDR_CTRL_REG1 0x20 /* sample rate constants are in the public header */
#define REG1_POWER_NORMAL (1<<3)
#define REG1_Z_ENABLE (1<<2)
#define REG1_Y_ENABLE (1<<1)
#define REG1_X_ENABLE (1<<0)
#define ADDR_CTRL_REG2 0x21
/* high-pass filter - usefulness TBD */
#define ADDR_CTRL_REG3 0x22
#define ADDR_CTRL_REG4 0x23
#define REG4_BDU (1<<7)
#define REG4_BIG_ENDIAN (1<<6)
#define REG4_SPI_3WIRE (1<<0)
#define ADDR_CTRL_REG5 0x24
#define REG5_BOOT (1<<7)
#define REG5_FIFO_EN (1<<6)
#define REG5_HIGHPASS_ENABLE (1<<4)
#define ADDR_REFERENCE 0x25
#define ADDR_TEMPERATURE 0x26
#define ADDR_STATUS_REG 0x27
#define STATUS_ZYXOR (1<<7)
#define SATAUS_ZOR (1<<6)
#define STATUS_YOR (1<<5)
#define STATUS_XOR (1<<4)
#define STATUS_ZYXDA (1<<3)
#define STATUS_ZDA (1<<2)
#define STATUS_YDA (1<<1)
#define STATUS_XDA (1<<0)
#define ADDR_OUT_X 0x28 /* 16 bits */
#define ADDR_OUT_Y 0x2A /* 16 bits */
#define ADDR_OUT_Z 0x2C /* 16 bits */
#define ADDR_FIFO_CTRL 0x2e
#define FIFO_MODE_BYPASS (0<<5)
#define FIFO_MODE_FIFO (1<<5)
#define FIFO_MODE_STREAM (2<<5)
#define FIFO_MODE_STREAM_TO_FIFO (3<<5)
#define FIFO_MODE_BYPASS_TO_STREAM (4<<5)
#define FIFO_THRESHOLD_MASK 0x1f
#define ADDR_FIFO_SRC 0x2f
#define FIFO_THREHSHOLD_OVER (1<<7)
#define FIFO_OVERRUN (1<<6)
#define FIFO_EMPTY (1<<5)
#define L3G4200_RATE_100Hz ((0<<6) | (0<<4))
#define L3G4200_RATE_200Hz ((1<<6) | (0<<4))
#define L3G4200_RATE_400Hz ((2<<6) | (1<<4))
#define L3G4200_RATE_800Hz ((3<<6) | (2<<4))
#define L3G4200_RANGE_250dps (0<<4)
#define L3G4200_RANGE_500dps (1<<4)
#define L3G4200_RANGE_2000dps (3<<4)
static void
write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data)
{
uint8_t cmd[2] = { address | DIR_WRITE, data };
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_SNDBLOCK(spi, &cmd, sizeof(cmd));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
}
static uint8_t
read_reg(struct spi_dev_s *spi, uint8_t address)
{
uint8_t cmd[2] = {address | DIR_READ, 0};
uint8_t data[2];
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, cmd, data, sizeof(cmd));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
return data[1];
}
int
l3gd20_test_configure(struct spi_dev_s *spi)
{
uint8_t id;
id = read_reg(spi, ADDR_WHO_AM_I);
if (id == WHO_I_AM)
{
message("L3GD20 SUCCESS: 0x%02x\n", id);
}
else
{
message("L3GD20 FAIL: 0x%02x\n", id);
}
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report;
report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
write_reg(spi, ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(spi, ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(spi, ADDR_CTRL_REG5, 0); /* turn off FIFO mode */
write_reg(spi, ADDR_CTRL_REG4, ((3<<4) & 0x30) | REG4_BDU);
write_reg(spi, ADDR_CTRL_REG1,
(((2<<6) | (1<<4)) & 0xf0) | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, &report, &report, sizeof(report));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
message("Init-read: gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z);
usleep(1000);
//message("got id 0x%02x, expected ID 0xd4\n", id);
return 0;
}
int
l3gd20_test_read(struct spi_dev_s *spi)
{
struct { /* status register and data as read back from the device */
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) report;
report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT;
SPI_LOCK(spi, true);
SPI_SELECT(spi, PX4_SPIDEV_GYRO, true);
SPI_EXCHANGE(spi, &report, &report, sizeof(report));
SPI_SELECT(spi, PX4_SPIDEV_GYRO, false);
SPI_LOCK(spi, false);
message("gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z);
usleep(1000);
return 0;
}
apps/px4/sensors_bringup/sensors.h
-88
View File
@@ -1,88 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef __APPS_PX4_SENSORS_H
#define __APPS_PX4_SENSORS_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/spi.h>
#include <nuttx/i2c.h>
/****************************************************************************
* Definitions
****************************************************************************/
/* Debug ********************************************************************/
#ifdef CONFIG_CPP_HAVE_VARARGS
# ifdef CONFIG_DEBUG
# define message(...) lib_rawprintf(__VA_ARGS__)
# define msgflush()
# else
# define message(...) printf(__VA_ARGS__)
# define msgflush() fflush(stdout)
# endif
#else
# ifdef CONFIG_DEBUG
# define message lib_rawprintf
# define msgflush()
# else
# define message printf
# define msgflush() fflush(stdout)
# endif
#endif
/****************************************************************************
* Public Types
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
int l3gd20_test_configure(struct spi_dev_s *spi);
int l3gd20_test_read(struct spi_dev_s *spi);
int bma180_test_configure(struct spi_dev_s *spi);
int bma180_test_read(struct spi_dev_s *spi);
int bma180_test(struct spi_dev_s *spi);
int hmc5883l_test(struct i2c_dev_s *i2c);
#endif /* __APPS_PX4_SENSORS_H */
apps/px4/sensors_bringup/sensors_main.c
-409
View File
@@ -1,409 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <arch/board/board.h>
#include <nuttx/spi.h>
#include <nuttx/i2c.h>
#include "sensors.h"
__EXPORT int sensors_bringup_main(int argc, char *argv[]);
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: user_start/adc_main
****************************************************************************/
int sensors_bringup_main(int argc, char *argv[])
{
struct spi_dev_s *spi;
int result = -1;
spi = up_spiinitialize(1);
if (!spi) {
message("Failed to initialize SPI port 1\n");
goto out;
}
struct i2c_dev_s *i2c;
i2c = up_i2cinitialize(2);
if (!i2c) {
message("Failed to initialize I2C bus 2\n");
goto out;
}
int ret;
#define EEPROM_ADDRESS 0x50
#define HMC5883L_ADDRESS 0x1E
//uint8_t devaddr = EEPROM_ADDRESS;
I2C_SETFREQUENCY(i2c, 100000);
//
// uint8_t subaddr = 0x00;
// int ret = 0;
//
// // ATTEMPT HMC5883L CONFIG
// I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7);
// subaddr = 0x02; // mode register
// ret = I2C_WRITE(i2c, &subaddr, 0);
// if (ret < 0)
// {
// message("I2C_WRITE failed: %d\n", ret);
// }
// else
// {
// message("I2C_WRITE SUCCEEDED: %d\n", ret);
// }
//fflush(stdout);
//
//
//
#define STATUS_REGISTER 0x09 // Of HMC5883L
// ATTEMPT HMC5883L WRITE
I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7);
uint8_t cmd = 0x09;
uint8_t status_id[4] = {0, 0, 0, 0};
ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4);
if (ret >= 0 && status_id[1] == 'H' && status_id[2] == '4' && status_id[3] == '3')
{
message("HMC5883L identified, device status: %d\n", status_id[0]);
} else {
message("HMC5883L identification failed: %d\n", ret);
}
#define HMC5883L_ADDR_CONF_A 0x00
#define HMC5883L_ADDR_CONF_B 0x01
#define HMC5883L_ADDR_MODE 0x02
#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */
#define HMC5883L_AVERAGING_2 (1 << 5)
#define HMC5883L_AVERAGING_4 (2 << 5)
#define HMC5883L_AVERAGING_8 (3 << 5)
#define HMC5883L_RATE_75HZ (6 << 2) /* 75 Hz */
#define HMC5883L_RANGE_0_88GA (0 << 5)
uint8_t rate_cmd[] = {HMC5883L_ADDR_CONF_A, HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8};
ret = I2C_WRITE(i2c, rate_cmd, sizeof(rate_cmd));
message("Wrote %d into register 0x00 of HMC, result: %d (0 = success)\n", HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8, ret);
uint8_t range_cmd[] = {HMC5883L_ADDR_CONF_B, HMC5883L_RANGE_0_88GA};
ret = I2C_WRITE(i2c, range_cmd, sizeof(range_cmd));
message("Wrote %d into register 0x01 of HMC, result: %d (0 = success)\n", HMC5883L_RANGE_0_88GA, ret);
// Set HMC into continous mode
// First write address, then value
uint8_t cont_address[] = {HMC5883L_ADDR_MODE, 0x00};
ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address));
message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret);
// ATTEMPT HMC5883L READ
int h = 0;
I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7);
for (h = 0; h < 5; h++)
{
cont_address[0] = HMC5883L_ADDR_MODE;
cont_address[1] = 0x01;
ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address));
message("Wrote 0x01 into register 0x02 of HMC, result: %d (0 = success)\n", ret);
usleep(100000);
cont_address[1] = 0x00;
uint8_t dummy;
ret = I2C_WRITEREAD(i2c, cont_address, sizeof(cont_address), &dummy, 1);
message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret);
usleep(100000);
int16_t hmc5883l_data[3] = {0, 0, 0};
uint8_t data_address = 0x03;
uint8_t* data_ptr = (uint8_t*)hmc5883l_data;
ret = I2C_WRITEREAD(i2c, &data_address, 1, data_ptr, 6);
if (ret < 0)
{
message("HMC5883L READ failed: %d\n", ret);
}
else
{
// mask out top four bits as only 12 bits are valid
hmc5883l_data[0] &= 0xFFF;
hmc5883l_data[1] &= 0xFFF;
hmc5883l_data[2] &= 0xFFF;
message("HMC5883L READ SUCCEEDED: %d, val: %d %d %d\n", ret, hmc5883l_data[0], hmc5883l_data[1], hmc5883l_data[2]);
uint8_t hmc_status;
ret = I2C_WRITEREAD(i2c, &cmd, 1, &hmc_status, 1);
message("\t status: %d\n", hmc_status);
}
}
// Possible addresses: 0x77 or 0x76
#define MS5611_ADDRESS_1 0x76
#define MS5611_ADDRESS_2 0x77
I2C_SETADDRESS(i2c, MS5611_ADDRESS_1, 7);
// Reset cmd
uint8_t ms5611_cmd[2] = {0x00, 0x1E};
ret = I2C_WRITE(i2c, ms5611_cmd, 2);
if (ret < 0)
{
message("MS5611 #1 WRITE failed: %d\n", ret);
}
else
{
message("MS5611 #1 WRITE SUCCEEDED: %d\n", ret);
}
fflush(stdout);
I2C_SETADDRESS(i2c, MS5611_ADDRESS_2, 7);
ret = I2C_WRITE(i2c, ms5611_cmd, 2);
if (ret < 0)
{
message("MS5611 #2 WRITE failed: %d\n", ret);
}
else
{
message("MS5611 #2 WRITE SUCCEEDED: %d\n", ret);
}
fflush(stdout);
// Wait for reset to complete (10 ms nominal, wait: 100 ms)
usleep(100000);
// Read PROM data
uint8_t prom_buf[2] = {0,1};
uint16_t calibration[6];
int i = 0;
prom_buf[0] = 0xA2 + (i*2);
struct i2c_msg_s msgv[2] = {
{
.addr = MS5611_ADDRESS_2,
.flags = 0,
.buffer = prom_buf,
.length = 1
},
{
.addr = MS5611_ADDRESS_2,
.flags = I2C_M_READ,
.buffer = prom_buf,
.length = 1
}
};
calibration[i] = prom_buf[0]*256;
calibration[i]+= prom_buf[1];
int retval;
if ( (retval = I2C_TRANSFER(i2c, msgv, 2)) == OK )
{
printf("SUCCESS ACCESSING PROM OF MS5611: %d, value C1: %d\n", retval, (int)calibration[0]);
}
else
{
printf("FAIL ACCESSING PROM OF MS5611\n");
}
// TESTING CODE, EEPROM READ/WRITE
uint8_t val[1] = {10};
int retval_eeprom;
uint8_t eeprom_subaddr[2] = {0, 0};
struct i2c_msg_s msgv_eeprom[2] = {
{
.addr = EEPROM_ADDRESS,
.flags = 0,
.buffer = eeprom_subaddr,
.length = 2
},
{
.addr = EEPROM_ADDRESS,
.flags = I2C_M_READ,
.buffer = val,
.length = 1
}
};
val[0] = 5;
if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom, 2)) == OK )
{
printf("SUCCESS READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
}
else
{
printf("FAIL READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
}
// Increment val
val[0] = val[0] + 1;
struct i2c_msg_s msgv_eeprom_write[2] = {
{
.addr = EEPROM_ADDRESS,
.flags = I2C_M_NORESTART,
.buffer = eeprom_subaddr,
.length = 2
},
{
.addr = EEPROM_ADDRESS,
.flags = I2C_M_NORESTART,
.buffer = val,
.length = 1
}
};
if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom_write, 2)) == OK )
{
printf("SUCCESS WRITING EEPROM: %d\n", retval_eeprom);
}
usleep(10000);
struct i2c_msg_s msgv_eeprom2[2] = {
{
.addr = EEPROM_ADDRESS,
.flags = 0,
.buffer = eeprom_subaddr,
.length = 2
},
{
.addr = EEPROM_ADDRESS,
.flags = I2C_M_READ,
.buffer = val,
.length = 1
}
};
val[0] = 5;
if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom2, 2)) == OK )
{
printf("SUCCESS READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
}
else
{
printf("FAIL READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]);
}
// Configure sensors
l3gd20_test_configure(spi);
bma180_test_configure(spi);
for (int i = 0; i < 3; i++)
{
l3gd20_test_read(spi);
bma180_test_read(spi);
printf("# %d of 10\n", i+1);
usleep(50000);
}
out:
msgflush();
return result;
}
apps/px4/tests/test_sensors.c
+20 -19
View File
@@ -60,7 +60,7 @@
#include <arch/board/drv_bma180.h>
#include <arch/board/drv_l3gd20.h>
#include <arch/board/drv_hmc5883l.h>
#include <arch/board/drv_mpu6000.h>
#include <drivers/drv_accel.h>
/****************************************************************************
* Pre-processor Definitions
@@ -94,7 +94,7 @@ struct {
{"bma180", "/dev/bma180", bma180},
{"hmc5883l", "/dev/hmc5883l", hmc5883l},
{"ms5611", "/dev/ms5611", ms5611},
{"mpu6000", "/dev/mpu6000", mpu6000},
{"mpu6000", "/dev/accel", mpu6000},
// {"lis331", "/dev/lis331", lis331},
{NULL, NULL, NULL}
};
@@ -331,13 +331,13 @@ mpu6000(int argc, char *argv[])
fflush(stdout);
int fd;
int16_t buf[6] = { -1, 0, -1, 0, -1, 0};
struct accel_report buf;
int ret;
fd = open("/dev/mpu6000", O_RDONLY);
fd = open("/dev/accel", O_RDONLY);
if (fd < 0) {
printf("\tMPU-6000: open fail\n");
printf("\tMPU-6000: open fail, run <mpu6000 start> first.\n");
return ERROR;
}
@@ -345,28 +345,28 @@ mpu6000(int argc, char *argv[])
usleep(100000);
/* read data - expect samples */
ret = read(fd, buf, sizeof(buf));
ret = read(fd, &buf, sizeof(buf));
if (ret != sizeof(buf)) {
if (ret < 3) {
printf("\tMPU-6000: read1 fail (%d)\n", ret);
return ERROR;
} else {
printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
printf("\tMPU-6000 values: acc: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);//\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
}
/* wait at least 10ms, sensor should have data after no more than 2ms */
usleep(100000);
// /* wait at least 10ms, sensor should have data after no more than 2ms */
// usleep(100000);
ret = read(fd, buf, sizeof(buf));
// ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
printf("\tMPU-6000: read2 fail (%d)\n", ret);
return ERROR;
// if (ret != sizeof(buf)) {
// printf("\tMPU-6000: read2 fail (%d)\n", ret);
// return ERROR;
} else {
printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
}
// } else {
// printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
// }
/* XXX more tests here */
@@ -398,7 +398,8 @@ ms5611(int argc, char *argv[])
ret = read(fd, buf, sizeof(buf));
if (ret != sizeof(buf)) {
if ((uint8_t)ret == -EAGAIN || (uint8_t)ret == -EINPROGRESS || i < 3) {
if ((int8_t)ret == -EAGAIN || (int8_t)ret == -EINPROGRESS) {
/* waiting for device to become ready, this is not an error */
} else {
printf("\tMS5611: read fail (%d)\n", ret);
@@ -435,7 +436,7 @@ hmc5883l(int argc, char *argv[])
fflush(stdout);
int fd;
int16_t buf[3] = {0, 0, 0};
int16_t buf[7] = {0, 0, 0};
int ret;
fd = open("/dev/hmc5883l", O_RDONLY);
apps/px4io/Makefile
+11
View File
@@ -35,4 +35,15 @@
# Build the px4io application.
#
#
# Note that we pull a couple of specific files from the systemlib, since
# we can't support it all.
#
CSRCS = comms.c \
mixer.c \
px4io.c \
safety.c \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c
include $(APPDIR)/mk/app.mk
apps/px4io/protocol.h
+6 -2
View File
@@ -48,6 +48,8 @@
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 2
#pragma pack(push, 1)
/* command from FMU to IO */
struct px4io_command {
uint16_t f2i_magic;
@@ -56,7 +58,7 @@ struct px4io_command {
uint16_t servo_command[PX4IO_OUTPUT_CHANNELS];
bool relay_state[PX4IO_RELAY_CHANNELS];
bool arm_ok;
} __attribute__((packed));
};
/* report from IO to FMU */
struct px4io_report {
@@ -66,4 +68,6 @@ struct px4io_report {
uint16_t rc_channel[PX4IO_INPUT_CHANNELS];
bool armed;
uint8_t channel_count;
} __attribute__((packed));
};
#pragma pack(pop)
apps/sensors/Makefile
+1 -1
View File
@@ -37,6 +37,6 @@
APPNAME = sensors
PRIORITY = SCHED_PRIORITY_MAX-5
STACKSIZE = 2048
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/sensors/sensors.c
+84 -21
View File
@@ -105,11 +105,15 @@ static pthread_mutex_t sensors_read_ready_mutex;
static int sensors_timer_loop_counter = 0;
/* File descriptors for all sensors */
static int fd_gyro = -1;
static int fd_accelerometer = -1;
static int fd_magnetometer = -1;
static int fd_barometer = -1;
static int fd_adc = -1;
static int fd_gyro = -1;
static int fd_accelerometer = -1;
static int fd_magnetometer = -1;
static int fd_barometer = -1;
static int fd_adc = -1;
static bool thread_should_exit = false;
static bool thread_running = false;
static int sensors_task;
/* Private functions declared static */
static void sensors_timer_loop(void *arg);
@@ -133,6 +137,11 @@ static int sensor_pub;
*/
__EXPORT int sensors_main(int argc, char *argv[]);
/**
* Print the usage
*/
static void usage(const char *reason);
/**
* Initialize all sensor drivers.
*
@@ -243,7 +252,7 @@ static void sensors_timer_loop(void *arg)
pthread_cond_broadcast(&sensors_read_ready);
}
int sensors_main(int argc, char *argv[])
int sensors_thread_main(int argc, char *argv[])
{
/* inform about start */
printf("[sensors] Initializing..\n");
@@ -315,7 +324,7 @@ int sensors_main(int argc, char *argv[])
float buf_barometer[3];
int16_t mag_offset[3] = {0, 0, 0};
int16_t acc_offset[3] = {0, 0, 0};
int16_t acc_offset[3] = {200, 0, 0};
int16_t gyro_offset[3] = {0, 0, 0};
bool mag_calibration_enabled = false;
@@ -422,6 +431,8 @@ int sensors_main(int argc, char *argv[])
/* Enable high resolution timer callback to unblock main thread, run after 2 ms */
hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL);
thread_running = true;
while (1) {
pthread_mutex_lock(&sensors_read_ready_mutex);
@@ -452,7 +463,7 @@ int sensors_main(int argc, char *argv[])
/* switching from non-HIL to HIL mode */
//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) {
if (vstatus.flag_hil_enabled && !hil_enabled) {
hil_enabled = true;
publishing = false;
int ret = close(sensor_pub);
@@ -759,9 +770,9 @@ int sensors_main(int argc, char *argv[])
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
/* throttle input */
manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled;
manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* mode switch input */
manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled;
@@ -781,10 +792,10 @@ int sensors_main(int argc, char *argv[])
if (gyro_updated) {
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor
raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor
raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor
raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor
/* scale measurements */
// XXX request scaling from driver instead of hardcoding it
@@ -802,15 +813,15 @@ int sensors_main(int argc, char *argv[])
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor
raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32768 : buf_accelerometer[0]; // y on the board is x of the sensor
raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : buf_accelerometer[2]; // z of the board is z of the sensor
// XXX read range from sensor
float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) / 8192.0f) * range_g) * 9.81f;
raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) / 8192.0f) * range_g) * 9.81f;
raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) / 8192.0f) * range_g) * 9.81f;
raw.accelerometer_raw_counter++;
}
@@ -820,9 +831,9 @@ int sensors_main(int argc, char *argv[])
/* copy sensor readings to global data and transform coordinates into px4fmu board frame */
/* assign negated value, except for -SHORT_MAX, as it would wrap there */
raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor
raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor
raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor
raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is y of the sensor
raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is -x of the sensor
raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor
// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f;
@@ -852,7 +863,7 @@ int sensors_main(int argc, char *argv[])
if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
/* Voltage in volts */
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (uint16_t)(buf_adc.am_data1 * battery_voltage_conversion)));
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * battery_voltage_conversion)));
if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
raw.battery_voltage_valid = false;
@@ -906,6 +917,8 @@ int sensors_main(int argc, char *argv[])
#endif
}
if (thread_should_exit) break;
}
/* Never really getting here */
@@ -919,6 +932,56 @@ int sensors_main(int argc, char *argv[])
printf("[sensors] exiting.\n");
thread_running = false;
return ret;
}
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: sensors {start|stop|status}\n");
exit(1);
}
int sensors_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("sensors app already running\n");
} else {
thread_should_exit = false;
sensors_task = task_create("sensors", SCHED_PRIORITY_MAX - 5, 4096, sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (!thread_running) {
printf("sensors app not started\n");
} else {
printf("stopping sensors app");
thread_should_exit = true;
}
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tsensors app is running\n");
} else {
printf("\tsensors app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
apps/systemcmds/top/top.c
+3 -3
View File
@@ -135,9 +135,9 @@ int top_main(int argc, char *argv[])
memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
#if CONFIG_RR_INTERVAL > 0
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
#else
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces);
printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces);
#endif
} else {
@@ -190,7 +190,7 @@ int top_main(int argc, char *argv[])
runtime_spaces = "";
}
printf("\033[K % 2d\t%s%s\t% 8lld ms%s \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]);
printf("\033[K % 2d\t%s%s % 8lld ms%s \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]);
/* Print scheduling info with RR time slice */
#if CONFIG_RR_INTERVAL > 0
printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
apps/systemlib/Makefile
+2 -1
View File
@@ -35,7 +35,8 @@
# System utility library
#
CSRCS = hx_stream.c \
CSRCS = err.c \
hx_stream.c \
perf_counter.c
#
apps/systemlib/err.c
+189
View File
@@ -0,0 +1,189 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file err.c
*
* Simple error/warning functions, heavily inspired by the BSD functions of
* the same names.
*/
#include <nuttx/config.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include "err.h"
#define NOCODE 1000 /* larger than maximum errno */
#if CONFIG_NFILE_STREAMS > 0
# include <stdio.h>
#elif defined(CONFIG_ARCH_LOWPUTC)
# include <debug.h>
extern int lib_lowvprintf(const char *fmt, va_list ap);
#else
# warning Cannot output without one of CONFIG_NFILE_STREAMS or CONFIG_ARCH_LOWPUTC
#endif
const char *
getprogname(void)
{
#if CONFIG_TASK_NAME_SIZE > 0
_TCB *thisproc = sched_self();
return thisproc->name;
#else
return "app";
#endif
}
static void
warnerr_core(int errcode, const char *fmt, va_list args)
{
#if CONFIG_NFILE_STREAMS > 0
fprintf(stderr, "%s: ", getprogname());
vfprintf(stderr, fmt, args);
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
errcode = -errcode;
if (errcode < NOCODE)
fprintf(stderr, ": %s", strerror(errcode));
fprintf(stderr, "\n");
#elif CONFIG_ARCH_LOWPUTC
lib_lowprintf("%s: ", getprogname());
lib_lowvprintf(fmt, args);
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
errcode = -errcode;
if (errcode < NOCODE)
lib_lowprintf(": %s", strerror(errcode));
lib_lowprintf("\n");
#endif
}
void
err(int exitcode, const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
verr(exitcode, fmt, args);
}
void
verr(int exitcode, const char *fmt, va_list args)
{
warnerr_core(errno, fmt, args);
exit(exitcode);
}
void
errc(int exitcode, int errcode, const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
verrc(exitcode, errcode, fmt, args);
}
void
verrc(int exitcode, int errcode, const char *fmt, va_list args)
{
warnerr_core(errcode, fmt, args);
exit(exitcode);
}
void
errx(int exitcode, const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
verrx(exitcode, fmt, args);
}
void
verrx(int exitcode, const char *fmt, va_list args)
{
warnerr_core(NOCODE, fmt, args);
exit(exitcode);
}
void
warn(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
vwarn(fmt, args);
}
void
vwarn(const char *fmt, va_list args)
{
warnerr_core(NOCODE, fmt, args);
}
void
warnc(int errcode, const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
vwarnc(errcode, fmt, args);
}
void
vwarnc(int errcode, const char *fmt, va_list args)
{
warnerr_core(errcode, fmt, args);
}
void
warnx(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
vwarnx(fmt, args);
}
void
vwarnx(const char *fmt, va_list args)
{
warnerr_core(NOCODE, fmt, args);
}
apps/px4/px4io/protocol.h → apps/systemlib/err.h
+22 -30
View File
@@ -32,42 +32,34 @@
****************************************************************************/
/**
* @file PX4FMU <-> PX4IO messaging protocol.
* @file err.h
*
* This initial version of the protocol is very simple; each side transmits a
* complete update with each frame. This avoids the sending of many small
* messages and the corresponding complexity involved.
* Simple error/warning functions, heavily inspired by the BSD functions of
* the same names.
*/
/*
* XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
* TREES ARE MERGED.
*/
#ifndef _SYSTEMLIB_ERR_H
#define _SYSTEMLIB_ERR_H
#define PX4IO_OUTPUT_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 2
#include <stdarg.h>
#pragma pack(push,1)
__BEGIN_DECLS
/* command from FMU to IO */
struct px4io_command {
uint16_t f2i_magic;
#define F2I_MAGIC 0x636d
__EXPORT const char *getprogname(void);
uint16_t servo_command[PX4IO_OUTPUT_CHANNELS];
bool relay_state[PX4IO_RELAY_CHANNELS];
bool arm_ok;
};
__EXPORT void err(int, const char *, ...) __attribute__((noreturn,format(printf,2, 3)));
__EXPORT void verr(int, const char *, va_list) __attribute__((noreturn,format(printf,2, 0)));
__EXPORT void errc(int, int, const char *, ...) __attribute__((noreturn,format(printf,3, 4)));
__EXPORT void verrc(int, int, const char *, va_list) __attribute__((noreturn,format(printf,3, 0)));
__EXPORT void errx(int, const char *, ...) __attribute__((noreturn,format(printf,2, 3)));
__EXPORT void verrx(int, const char *, va_list) __attribute__((noreturn,format(printf,2, 0)));
__EXPORT void warn(const char *, ...) __attribute__((format(printf,1, 2)));
__EXPORT void vwarn(const char *, va_list) __attribute__((format(printf,1, 0)));
__EXPORT void warnc(int, const char *, ...) __attribute__((format(printf,2, 3)));
__EXPORT void vwarnc(int, const char *, va_list) __attribute__((format(printf,2, 0)));
__EXPORT void warnx(const char *, ...) __attribute__((format(printf,1, 2)));
__EXPORT void vwarnx(const char *, va_list) __attribute__((format(printf,1, 0)));
/* report from IO to FMU */
struct px4io_report {
uint16_t i2f_magic;
#define I2F_MAGIC 0x7570
__END_DECLS
uint16_t rc_channel[PX4IO_INPUT_CHANNELS];
bool armed;
uint8_t channel_count;
} __attribute__((packed));
#pragma pack(pop)
#endif
apps/systemlib/mixer/mixer_multirotor.cpp
+1 -1
View File
@@ -179,7 +179,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
fixup_scale = 2.0f;
}
for (unsigned i = 0; i < _rotor_count; i++)
outputs[i] *= -1.0 + (outputs[i] * fixup_scale);
outputs[i] = -1.0 + (outputs[i] * fixup_scale);
/* ensure outputs are out of the deadband */
for (unsigned i = 0; i < _rotor_count; i++)
apps/systemlib/systemlib.c
+16 -7
View File
@@ -123,6 +123,11 @@ void kill_task(FAR _TCB *tcb, FAR void *arg)
kill(tcb->pid, SIGUSR1);
}
union param_union {
float f;
char c[4];
};
int store_params_in_eeprom(struct global_data_parameter_storage_t *params)
{
int ret = ERROR;
@@ -147,10 +152,13 @@ int store_params_in_eeprom(struct global_data_parameter_storage_t *params)
ret = ERROR;
} else {
for (int i = 0; i < params->pm.size; i++) {
write_res = write(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i]));
for (int i = 0; i < PARAM_MAX_COUNT; i++) {
if (write_res != sizeof(params->pm.param_values[i])) return ERROR;
union param_union p;
p.f = params->pm.param_values[i];
write_res = write(fd, p.c, sizeof(p.f));
if (write_res != sizeof(p.f)) return ERROR;
}
/*Write end magic byte */
@@ -221,10 +229,11 @@ int get_params_from_eeprom(struct global_data_parameter_storage_t *params)
/* read data */
if (lseek_res == OK) {
for (int i = 0; i < params->pm.size; i++) {
read_res = read(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i]));
if (read_res != sizeof(params->pm.param_values[i])) return ERROR;
for (int i = 0; i < PARAM_MAX_COUNT; i++) {
union param_union p;
read_res = read(fd, p.c, sizeof(p.f));
params->pm.param_values[i] = p.f;
if (read_res != sizeof(p.f)) return ERROR;
}
ret = OK;
apps/uORB/parameter_storage.h
+6 -2
View File
@@ -112,25 +112,28 @@ enum PARAM {
PARAM_MAX_COUNT ///< LEAVE THIS IN PLACE AS LAST ELEMENT!
};
#pragma pack(push, 1)
struct px4_parameter_storage_t {
float param_values[PARAM_MAX_COUNT]; ///< Parameter values
const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names
bool param_needs_write[PARAM_MAX_COUNT];
const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names
uint16_t next_param;
uint16_t size;
};
#pragma pack(pop)
#define PX4_FLIGHT_ENVIRONMENT_INDOOR 0
#define PX4_FLIGHT_ENVIRONMENT_OUTDOOR 1
#define PX4_FLIGHT_ENVIRONMENT_TESTING 2 //NO check for position fix in this environment
#pragma pack(push, 1)
struct global_data_parameter_storage_t {
/* use of a counter and timestamp recommended (but not necessary) */
// uint16_t counter; //incremented by the writing thread everytime new data is stored
// uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
/* Actual data, this is specific to the type of data which is stored in this struct */
@@ -142,6 +145,7 @@ struct global_data_parameter_storage_t {
//*****END: Add your variables here *****
};
#pragma pack(pop)
__attribute__ ((visibility ("default"))) extern struct global_data_parameter_storage_t *global_data_parameter_storage; //adjust this line!
apps/uORB/topics/vehicle_status.h
+27 -13
View File
@@ -87,9 +87,16 @@ enum VEHICLE_MODE_FLAG {
}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
enum VEHICLE_FLIGHT_MODE {
VEHICLE_FLIGHT_MODE_MANUAL,
VEHICLE_FLIGHT_MODE_STABILIZED,
VEHICLE_FLIGHT_MODE_AUTO
VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */
VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */
VEHICLE_FLIGHT_MODE_STABILIZED, /**< attitude or rate stabilization plus velocity or position stabilization */
VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */
};
enum VEHICLE_ATTITUDE_MODE {
VEHICLE_ATTITUDE_MODE_DIRECT, /**< no attitude control, direct stick input mixing (only fixed wing) */
VEHICLE_ATTITUDE_MODE_RATES, /**< body rates control mode */
VEHICLE_ATTITUDE_MODE_ATTITUDE /**< tait-bryan attitude control mode */
};
/**
@@ -106,14 +113,21 @@ struct vehicle_status_s
commander_state_machine_t state_machine; /**< Current flight state, main state machine */
enum VEHICLE_FLIGHT_MODE flight_mode; /**< Current flight mode, as defined by mode switch */
enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< Current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
uint8_t mode;
// uint8_t mode;
bool control_manual_enabled; /**< true if manual input is mixed in */
bool control_rates_enabled; /**< true if rates are stabilized */
bool control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool control_speed_enabled; /**< true if speed (implies direction) is controlled */
bool control_position_enabled; /**< true if position is controlled */
/* system flags - these represent the state predicates */
bool flag_system_armed; /**< True is motors / actuators are armed */
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_hil_enabled; /**< True if hardware in the loop simulation is enabled */
// bool flag_control_rates_enabled; /**< true if rates are stabilized */
// bool flag_control_attitude_enabled; *< true if attitude stabilization is mixed in
// bool control_speed_enabled; /**< true if speed (implies direction) is controlled */
// bool control_position_enabled; /**< true if position is controlled */
bool preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool preflight_mag_calibration; /**< true if mag calibration is requested */
@@ -126,10 +140,10 @@ struct vehicle_status_s
uint32_t onboard_control_sensors_present;
uint32_t onboard_control_sensors_enabled;
uint32_t onboard_control_sensors_health;
uint16_t load;
uint16_t voltage_battery;
int16_t current_battery;
int8_t battery_remaining;
float load;
float voltage_battery;
float current_battery;
float battery_remaining;
uint16_t drop_rate_comm;
uint16_t errors_comm;
uint16_t errors_count1;
apps/uORB/uORB.cpp
+4 -3
View File
@@ -728,7 +728,8 @@ uorb_main(int argc, char *argv[])
if (g_dev != nullptr) {
fprintf(stderr, "[uorb] already loaded\n");
return -EBUSY;
/* user wanted to start uorb, its already running, no error */
return 0;
}
/* create the driver */
@@ -759,10 +760,10 @@ uorb_main(int argc, char *argv[])
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
if (!strcmp(argv[1], "status"))
return info();
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n");
return -EINVAL;
}
nuttx/arch/arm/src/arm/arm.h
-451
View File
@@ -1,451 +0,0 @@
/************************************************************************************
* arch/arm/src/arm/arm.h
*
* Copyright (C) 2007-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
************************************************************************************/
#ifndef __ARCH_ARM_SRC_COMMON_ARM_H
#define __ARCH_ARM_SRC_COMMON_ARM_H
/************************************************************************************
* Included Files
************************************************************************************/
/************************************************************************************
* Pre-processor Definitions
************************************************************************************/
#undef CONFIG_ALIGNMENT_TRAP
#undef CONFIG_DCACHE_WRITETHROUGH
#undef CONFIG_CACHE_ROUND_ROBIN
#undef CONFIG_DCACHE_DISABLE
#undef CONFIG_ICACHE_DISABLE
/* ARM9EJS **************************************************************************/
/* PSR bits */
#define MODE_MASK 0x0000001f /* Bits 0-4: Mode bits */
# define USR26_MODE 0x00000000 /* 26-bit User mode */
# define FIQ26_MODE 0x00000001 /* 26-bit FIQ mode */
# define IRQ26_MODE 0x00000002 /* 26-bit IRQ mode */
# define SVC26_MODE 0x00000003 /* 26-bit Supervisor mode */
# define MODE32_BIT 0x00000010 /* Bit 4: 32-bit mode */
# define USR_MODE 0x00000010 /* 32-bit User mode */
# define FIQ_MODE 0x00000011 /* 32-bit FIQ mode */
# define IRQ_MODE 0x00000012 /* 32-bit IRQ mode */
# define SVC_MODE 0x00000013 /* 32-bit Supervisor mode */
# define ABT_MODE 0x00000017 /* 32-bit Abort mode */
# define UND_MODE 0x0000001b /* 32-bit Undefined mode */
# define SYSTEM_MODE 0x0000001f /* 32-bit System mode */
#define PSR_T_BIT 0x00000020 /* Bit 5: Thumb state */
#define PSR_F_BIT 0x00000040 /* Bit 6: FIQ disable */
#define PSR_I_BIT 0x00000080 /* Bit 7: IRQ disable */
/* Bits 8-23: Reserved */
#define PSR_J_BIT 0x01000000 /* Bit 24: Jazelle state bit */
/* Bits 25-26: Reserved */
#define PSR_Q_BIT 0x08000000 /* Bit 27: Sticky overflow */
#define PSR_V_BIT 0x10000000 /* Bit 28: Overflow */
#define PSR_C_BIT 0x20000000 /* Bit 29: Carry/Borrow/Extend */
#define PSR_Z_BIT 0x40000000 /* Bit 30: Zero */
#define PSR_N_BIT 0x80000000 /* Bit 31: Negative/Less than */
/* CR1 bits (CP#15 CR1) */
#define CR_M 0x00000001 /* MMU enable */
#define CR_A 0x00000002 /* Alignment abort enable */
#define CR_C 0x00000004 /* Dcache enable */
#define CR_W 0x00000008 /* Write buffer enable */
#define CR_P 0x00000010 /* 32-bit exception handler */
#define CR_D 0x00000020 /* 32-bit data address range */
#define CR_L 0x00000040 /* Implementation defined */
#define CR_B 0x00000080 /* Big endian */
#define CR_S 0x00000100 /* System MMU protection */
#define CR_R 0x00000200 /* ROM MMU protection */
#define CR_F 0x00000400 /* Implementation defined */
#define CR_Z 0x00000800 /* Implementation defined */
#define CR_I 0x00001000 /* Icache enable */
#define CR_V 0x00002000 /* Vectors relocated to 0xffff0000 */
#define CR_RR 0x00004000 /* Round Robin cache replacement */
#define CR_L4 0x00008000 /* LDR pc can set T bit */
#define CR_DT 0x00010000
#define CR_IT 0x00040000
#define CR_ST 0x00080000
#define CR_FI 0x00200000 /* Fast interrupt (lower latency mode) */
#define CR_U 0x00400000 /* Unaligned access operation */
#define CR_XP 0x00800000 /* Extended page tables */
#define CR_VE 0x01000000 /* Vectored interrupts */
/* The lowest 4-bits of the FSR register indicates the fault generated by
* the MMU.
*/
#define FSR_MASK 15 /* Bits 0-3: Type of fault */
#define FSR_VECTOR 0 /* Vector exception */
#define FSR_ALIGN1 1 /* Alignment fault */
#define FSR_TERMINAL 2 /* Terminal exception */
#define FSR_ALIGN2 3 /* Alignment fault */
#define FSR_LINESECT 4 /* External abort on linefetch for section translation */
#define FSR_SECT 5 /* Section translation fault (unmapped virtual address) */
#define FSR_LINEPAGE 6 /* External abort on linefetch for page translation */
#define FSR_PAGE 7 /* Page translation fault (unmapped virtual address) */
#define FSR_NLINESECT 8 /* External abort on non-linefetch for section translation */
#define FSR_DOMSECT 9 /* Domain fault on section translation (i.e. accessing invalid domain) */
#define FSR_NLINEPAGE 10 /* External abort on non-linefetch for page translation */
#define FSR_DOMPAGE 11 /* Domain fault on page translation (i.e. accessing invalid domain) */
#define FSR_EXTERN1 12 /* External abort on first level translation */
#define FSR_PERMSECT 13 /* Permission fault on section (i.e. no permission to access virtual address) */
#define FSR_EXTERN2 14 /* External abort on second level translation */
#define FSR_PERMPAGE 15 /* Permission fault on page (i.e. no permission to access virtual address) */
#define FSR_DOM_SHIFT 4 /* Bits 4-7: Domain */
#define FSR_DOM_MASK (15 << FSR_DOM_SHIFT)
/* Hardware page table definitions.
*
* Level 1 Descriptor (PMD)
*
* Common definitions.
*/
#define PMD_TYPE_MASK 0x00000003 /* Bits 1:0: Type of descriptor */
#define PMD_TYPE_FAULT 0x00000000
#define PMD_TYPE_COARSE 0x00000001
#define PMD_TYPE_SECT 0x00000002
#define PMD_TYPE_FINE 0x00000003
/* Bits 3:2: Depends on descriptor */
#define PMD_BIT4 0x00000010 /* Bit 4: Must be one */
#define PMD_DOMAIN_MASK 0x000001e0 /* Bits 8:5: Domain control bits */
#define PMD_DOMAIN(x) ((x) << 5)
#define PMD_PROTECTION 0x00000200 /* Bit 9: v5 only */
/* Bits 31:10: Depend on descriptor */
/* Level 1 Section Descriptor. Section descriptors allow fast, single
* level mapping between 1Mb address regions.
*/
/* Bits 1:0: Type of mapping */
#define PMD_SECT_BUFFERABLE 0x00000004 /* Bit 2: 1=bufferable */
#define PMD_SECT_CACHEABLE 0x00000008 /* Bit 3: 1=cacheable */
/* Bit 4: Common, must be one */
/* Bits 8:5: Common domain control */
/* Bit 9: Common protection */
#define PMD_SECT_AP_MASK 0x00000c00 /* Bits 11:10: Access permission */
#define PMD_SECT_AP_WRITE 0x00000400
#define PMD_SECT_AP_READ 0x00000800
/* Bits 19:20: Should be zero */
#define PMD_SECT_TEX_MASK 0xfff00000 /* Bits 31:20: v5, Physical page */
#define PMD_SECT_APX 0x00008000 /* Bit 15: v6 only */
#define PMD_SECT_S 0x00010000 /* Bit 16: v6 only */
#define PMD_SECT_nG 0x00020000 /* Bit 17: v6 only */
#define PMD_SECT_UNCACHED (0)
#define PMD_SECT_BUFFERED (PMD_SECT_BUFFERABLE)
#define PMD_SECT_WT (PMD_SECT_CACHEABLE)
#define PMD_SECT_WB (PMD_SECT_CACHEABLE|PMD_SECT_BUFFERABLE)
#define PMD_SECT_MINICACHE (PMD_SECT_TEX(1)|PMD_SECT_CACHEABLE)
#define PMD_SECT_WBWA (PMD_SECT_TEX(1)|PMD_SECT_CACHEABLE|PMD_SECT_BUFFERABLE)
/* Level 1 Coarse Table Descriptor. Coarse Table Descriptors support
* two level mapping between 16Kb memory regions.
*/
/* Bits 1:0: Type of mapping */
/* Bits 3:2: Should be zero */
/* Bit 4: Common, must be one */
/* Bits 8:5: Common domain control */
/* Bits 9: Should be zero */
#define PMD_COARSE_TEX_MASK 0xfffffc00 /* Bits 31:10: v5, Physical page */
/* Level 1 Fine Table Descriptor. Coarse Table Descriptors support
* two level mapping between 4Kb memory regions.
*/
/* Bits 1:0: Type of mapping */
/* Bits 3:2: Should be zero */
/* Bit 4: Common, must be one */
/* Bits 8:5: Common domain control */
/* Bits 11:9: Should be zero */
#define PMD_FINE_TEX_MASK 0xfffff000 /* Bits 31:12: v5, Physical page */
/* Level 2 Table Descriptor (PTE). A section descriptor provides the base address
* of a 1MB block of memory. The page table descriptors provide the base address of
* a page table that contains second-level descriptors. There are two sizes of page
* table:
* - Coarse page tables have 256 entries, splitting the 1MB that the table
* describes into 4KB blocks
* - Fine/tiny page tables have 1024 entries, splitting the 1MB that the table
* describes into 1KB blocks.
*
* The following definitions apply to all L2 tables:
*/
#define PTE_TYPE_MASK (3 << 0) /* Bits: 1:0: Type of mapping */
#define PTE_TYPE_FAULT (0 << 0) /* None */
#define PTE_TYPE_LARGE (1 << 0) /* 64Kb of memory */
#define PTE_TYPE_SMALL (2 << 0) /* 4Kb of memory */
#define PTE_TYPE_TINY (3 << 0) /* 1Kb of memory (v5)*/
#define PTE_BUFFERABLE (1 << 2) /* Bit 2: 1=bufferable */
#define PTE_CACHEABLE (1 << 3) /* Bit 3: 1=cacheable */
/* Bits 31:4: Depend on type */
/* Large page -- 64Kb */
/* Bits: 1:0: Type of mapping */
/* Bits: 3:2: Bufferable/cacheable */
#define PTE_LARGE_AP_MASK (0xff << 4) /* Bits 11:4 Access permissions */
#define PTE_LARGE_AP_UNO_SRO (0x00 << 4)
#define PTE_LARGE_AP_UNO_SRW (0x55 << 4)
#define PTE_LARGE_AP_URO_SRW (0xaa << 4)
#define PTE_LARGE_AP_URW_SRW (0xff << 4)
/* Bits 15:12: Should be zero */
#define PTE_LARGE_TEX_MASK 0xffff0000 /* Bits 31:16: v5, Physical page */
/* Small page -- 4Kb */
/* Bits: 1:0: Type of mapping */
/* Bits: 3:2: Bufferable/cacheable */
#define PTE_SMALL_AP_MASK (0xff << 4) /* Bits: 11:4: Access permissions */
#define PTE_SMALL_AP_UNO_SRO (0x00 << 4)
#define PTE_SMALL_AP_UNO_SRW (0x55 << 4)
#define PTE_SMALL_AP_URO_SRW (0xaa << 4)
#define PTE_SMALL_AP_URW_SRW (0xff << 4)
#define PTE_SMALL_TEX_MASK 0xfffff000 /* Bits: 31:12: Physical page */
#define PTE_SMALL_NPAGES 256 /* 256 Coarse PTE's per section */
/* Fine/Tiny page -- 1Kb */
/* Bits: 1:0: Type of mapping */
/* Bits: 3:2: Bufferable/cacheable */
#define PTE_EXT_AP_MASK (3 << 4) /* Bits: 5:4: Access persions */
#define PTE_EXT_AP_UNO_SRO (0 << 4)
#define PTE_EXT_AP_UNO_SRW (1 << 4)
#define PTE_EXT_AP_URO_SRW (2 << 4)
#define PTE_EXT_AP_URW_SRW (3 << 4)
/* Bits: 9:6: Should be zero */
#define PTE_TINY_TEX_MASK 0xfffffc00 /* Bits: 31:10: Physical page */
#define PTE_TINY_NPAGES 1024 /* 1024 Tiny PTE's per section */
/* Default MMU flags for RAM memory, IO, vector region */
#define MMU_ROMFLAGS \
(PMD_TYPE_SECT|PMD_BIT4|PMD_SECT_AP_READ)
#define MMU_MEMFLAGS \
(PMD_TYPE_SECT|PMD_SECT_WB|PMD_BIT4|PMD_SECT_AP_WRITE|PMD_SECT_AP_READ)
#define MMU_IOFLAGS \
(PMD_TYPE_SECT|PMD_BIT4|PMD_SECT_AP_WRITE|PMD_SECT_AP_READ)
#define MMU_L1_VECTORFLAGS (PMD_TYPE_COARSE|PMD_BIT4)
#define MMU_L2_VECTORFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
/* Mapped section size */
#define SECTION_SIZE (1 << 20) /* 1Mb */
/* CP15 register c2 contains a pointer to the base address of a paged table in
* physical memory. Only bits 14-31 of the page table address is retained there;
* The full 30-bit address is formed by ORing in bits 2-13 or the virtual address
* (MVA). As a consequence, the page table must be aligned to a 16Kb address in
* physical memory and could require up to 16Kb of memory.
*/
#define PGTABLE_SIZE 0x00004000
/************************************************************************************
* Inline Functions
************************************************************************************/
#ifndef __ASSEMBLY__
/* Get the current value of the CP15 C1 control register */
static inline unsigned int get_cp15c1(void)
{
unsigned int retval;
__asm__ __volatile__
(
"\tmrc p15, 0, %0, c1, c0"
: "=r" (retval)
:
: "memory");
return retval;
}
/* Get the current value of the CP15 C2 page table pointer register */
static inline unsigned int get_cp15c2(void)
{
unsigned int retval;
__asm__ __volatile__
(
"\tmrc p15, 0, %0, c2, c0"
: "=r" (retval)
:
: "memory");
return retval;
}
/* Get the current value of the CP15 C3 domain access register */
static inline unsigned int get_cp15c3(void)
{
unsigned int retval;
__asm__ __volatile__
(
"\tmrc p15, 0, %0, c3, c0"
: "=r" (retval)
:
: "memory");
return retval;
}
/* ARMv4/ARMv5 operation: Invalidate TLB
* ARM926EJ-S operation: Invalidate set-associative
* Data: Should be zero
*/
static inline void tlb_invalidate(void)
{
unsigned int sbz = 0;
__asm__ __volatile__
(
"\tmcr p15, 0, %0, c8, c7, 0"
:
: "r" (sbz)
: "memory");
}
/* ARMv4/ARMv5 operation: Invalidate TLB single entry (MVA)
* ARM926EJ-S operation: Invalidate single entry
* Data: MVA
*/
static inline void tlb_invalidate_single(unsigned int mva)
{
mva &= 0xfffffc00;
__asm__ __volatile__
(
"mcr p15, 0, %0, c8, c7, 1"
:
: "r" (mva)
: "memory");
}
/* ARMv4/ARMv5 operation: Invalidate instruction TLB
* ARM926EJ-S operation: Invalidate set-associative TLB
* Data: Should be zero
*/
static inline void tlb_instr_invalidate(void)
{
unsigned int sbz = 0;
__asm__ __volatile__
(
"\tmcr p15, 0, %0, c8, c5, 0"
:
: "r" (sbz)
: "memory");
}
/* ARMv4/ARMv5 operation: Invalidate instruction TLB single entry (MVA)
* ARM926EJ-S operation: Invalidate single entry
* Data: MVA
*/
static inline void tlb_inst_invalidate_single(unsigned int mva)
{
mva &= 0xfffffc00;
__asm__ __volatile__
(
"mcr p15, 0, %0, c8, c5, 1"
:
: "r" (mva)
: "memory");
}
/* ARMv4/ARMv5 operation: Invalidate data TLB
* ARM926EJ-S operation: Invalidate set-associative TLB
* Data: Should be zero
*/
static inline void tlb_data_invalidate(void)
{
unsigned int sbz = 0;
__asm__ __volatile__
(
"\tmcr p15, 0, %0, c8, c6, 0"
:
: "r" (sbz)
: "memory");
}
/* ARMv4/ARMv5 operation: Invalidate data TLB single entry (MVA)
* ARM926EJ-S operation: Invalidate single entry
* Data: MVA
*/
static inline void tlb_data_invalidate_single(unsigned int mva)
{
mva &= 0xfffffc00;
__asm__ __volatile__
(
"mcr p15, 0, %0, c8, c6, 1"
:
: "r" (mva)
: "memory");
}
#endif /* __ASSEMBLY__ */
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
#ifndef __ASSEMBLY__
#ifdef __cplusplus
#define EXTERN extern "C"
extern "C" {
#else
#define EXTERN extern
#endif
#undef EXTERN
#ifdef __cplusplus
}
#endif
#endif /* __ASSEMBLY__ */
#endif /* __ARCH_ARM_SRC_COMMON_ARM_H */
nuttx/arch/arm/src/arm/pg_macros.h
-522
View File
@@ -1,522 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/pg_macros.h
*
* Copyright (C) 2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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.
*
****************************************************************************/
/* Do not change this macro definition without making corresponding name
* changes in other files. This macro name is used in various places to
* assure that some file inclusion ordering dependencies are enforced.
*/
#ifndef __ARCH_ARM_SRC_ARM_PG_MACROS_H
#define __ARCH_ARM_SRC_ARM_PG_MACROS_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/page.h>
#include "arm.h"
/****************************************************************************
* Definitions
****************************************************************************/
/* Configuration ************************************************************/
#ifdef CONFIG_PAGING
/* Sanity check -- we cannot be using a ROM page table and supporting on-
* demand paging.
*/
#ifdef CONFIG_ARCH_ROMPGTABLE
# error "Cannot support both CONFIG_PAGING and CONFIG_ARCH_ROMPGTABLE"
#endif
/* Virtual Page Table Location **********************************************/
/* Check if the virtual address of the page table has been defined. It should
* not be defined: architecture specific logic should suppress defining
* PGTABLE_BASE_VADDR unless: (1) it is defined in the NuttX configuration
* file, or (2) the page table is position in low memory (because the vectors
* are in high memory).
*/
#ifndef PGTABLE_BASE_VADDR
# define PGTABLE_BASE_VADDR (PG_LOCKED_VBASE + PG_TEXT_VSIZE + PG_DATA_SIZE)
/* Virtual base of the address of the L2 page tables need to recalculates
* using this new virtual base address of the L2 page table.
*/
# undef PGTABLE_L2_FINE_VBASE
# define PGTABLE_L2_FINE_VBASE (PGTABLE_BASE_VADDR+PGTABLE_L2_FINE_OFFSET)
# undef PGTABLE_L2_COARSE_VBASE
# define PGTABLE_L2_COARSE_VBASE (PGTABLE_BASE_VADDR+PGTABLE_L2_COARSE_OFFSET)
#endif
/* Page Size Selections *****************************************************/
/* Create some friendly definitions to handle some differences between
* small and tiny pages.
*/
#if CONFIG_PAGING_PAGESIZE == 1024
/* Base of the L2 page table (aligned to 4Kb byte boundaries) */
# define PGTABLE_L2_BASE_PADDR PGTABLE_L2_FINE_PBASE
# define PGTABLE_L2_BASE_VADDR PGTABLE_L2_FINE_VBASE
/* Number of pages in an L2 table per L1 entry */
# define PTE_NPAGES PTE_TINY_NPAGES
/* Mask to get the page table physical address from an L1 entry */
# define PG_L1_PADDRMASK PMD_FINE_TEX_MASK
/* MMU Flags for each memory region */
# define MMU_L1_TEXTFLAGS (PMD_TYPE_FINE|PMD_BIT4)
# define MMU_L2_TEXTFLAGS (PTE_TYPE_TINY|PTE_EXT_AP_UNO_SRO|PTE_CACHEABLE)
# define MMU_L1_DATAFLAGS (PMD_TYPE_FINE|PMD_BIT4)
# define MMU_L2_DATAFLAGS (PTE_TYPE_TINY|PTE_EXT_AP_UNO_SRW|PTE_CACHEABLE|PTE_BUFFERABLE)
# define MMU_L2_ALLOCFLAGS (PTE_TYPE_TINY|PTE_EXT_AP_UNO_SRW)
# define MMU_L1_PGTABFLAGS (PMD_TYPE_FINE|PMD_BIT4)
# define MMU_L2_PGTABFLAGS (PTE_TYPE_TINY|PTE_EXT_AP_UNO_SRW)
# define MMU_L2_VECTRWFLAGS (PTE_TYPE_TINY|PTE_EXT_AP_UNO_SRW)
# define MMU_L2_VECTROFLAGS (PTE_TYPE_TINY|PTE_EXT_AP_UNO_SRO|PTE_CACHEABLE)
#elif CONFIG_PAGING_PAGESIZE == 4096
/* Base of the L2 page table (aligned to 1Kb byte boundaries) */
# define PGTABLE_L2_BASE_PADDR PGTABLE_L2_COARSE_PBASE
# define PGTABLE_L2_BASE_VADDR PGTABLE_L2_COARSE_VBASE
/* Number of pages in an L2 table per L1 entry */
# define PTE_NPAGES PTE_SMALL_NPAGES
/* Mask to get the page table physical address from an L1 entry */
# define PG_L1_PADDRMASK PMD_COARSE_TEX_MASK
/* MMU Flags for each memory region. */
# define MMU_L1_TEXTFLAGS (PMD_TYPE_COARSE|PMD_BIT4)
# define MMU_L2_TEXTFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRO|PTE_CACHEABLE)
# define MMU_L1_DATAFLAGS (PMD_TYPE_COARSE|PMD_BIT4)
# define MMU_L2_DATAFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW|PTE_CACHEABLE|PTE_BUFFERABLE)
# define MMU_L2_ALLOCFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
# define MMU_L1_PGTABFLAGS (PMD_TYPE_COARSE|PMD_BIT4)
# define MMU_L2_PGTABFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
# define MMU_L2_VECTRWFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW)
# define MMU_L2_VECTROFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRO|PTE_CACHEABLE)
#else
# error "Need extended definitions for CONFIG_PAGING_PAGESIZE"
#endif
#define PT_SIZE (4*PTE_NPAGES)
/* Addresses of Memory Regions **********************************************/
/* We position the locked region PTEs at an offset into the first
* L2 page table. The L1 entry points to an 1Mb aligned virtual
* address. The actual L2 entry will be offset into the aligned
* L2 table.
*
* Coarse: PG_L1_PADDRMASK=0xfffffc00
* OFFSET=(((a) & 0x000fffff) >> 12) << 2)
* Fine: PG_L1_PADDRMASK=0xfffff000
* OFFSET=(((a) & 0x000fffff) >> 10) << 2)
*/
#define PG_L1_LOCKED_PADDR (PGTABLE_BASE_PADDR + ((PG_LOCKED_VBASE >> 20) << 2))
#define PG_L1_LOCKED_VADDR (PGTABLE_BASE_VADDR + ((PG_LOCKED_VBASE >> 20) << 2))
#define PG_L2_LOCKED_OFFSET (((PG_LOCKED_VBASE & 0x000fffff) >> PAGESHIFT) << 2)
#define PG_L2_LOCKED_PADDR (PGTABLE_L2_BASE_PADDR + PG_L2_LOCKED_OFFSET)
#define PG_L2_LOCKED_VADDR (PGTABLE_L2_BASE_VADDR + PG_L2_LOCKED_OFFSET)
#define PG_L2_LOCKED_SIZE (4*CONFIG_PAGING_NLOCKED)
/* We position the paged region PTEs immediately after the locked
* region PTEs. NOTE that the size of the paged regions is much
* larger than the size of the physical paged region. That is the
* core of what the On-Demanding Paging feature provides.
*/
#define PG_L1_PAGED_PADDR (PGTABLE_BASE_PADDR + ((PG_PAGED_VBASE >> 20) << 2))
#define PG_L1_PAGED_VADDR (PGTABLE_BASE_VADDR + ((PG_PAGED_VBASE >> 20) << 2))
#define PG_L2_PAGED_PADDR (PG_L2_LOCKED_PADDR + PG_L2_LOCKED_SIZE)
#define PG_L2_PAGED_VADDR (PG_L2_LOCKED_VADDR + PG_L2_LOCKED_SIZE)
#define PG_L2_PAGED_SIZE (4*CONFIG_PAGING_NVPAGED)
/* This describes the overall text region */
#define PG_L1_TEXT_PADDR PG_L1_LOCKED_PADDR
#define PG_L1_TEXT_VADDR PG_L1_LOCKED_VADDR
#define PG_L2_TEXT_PADDR PG_L2_LOCKED_PADDR
#define PG_L2_TEXT_VADDR PG_L2_LOCKED_VADDR
#define PG_L2_TEXT_SIZE (PG_L2_LOCKED_SIZE + PG_L2_PAGED_SIZE)
/* We position the data section PTEs just after the text region PTE's */
#define PG_L1_DATA_PADDR (PGTABLE_BASE_PADDR + ((PG_DATA_VBASE >> 20) << 2))
#define PG_L1_DATA_VADDR (PGTABLE_BASE_VADDR + ((PG_DATA_VBASE >> 20) << 2))
#define PG_L2_DATA_PADDR (PG_L2_LOCKED_PADDR + PG_L2_TEXT_SIZE)
#define PG_L2_DATA_VADDR (PG_L2_LOCKED_VADDR + PG_L2_TEXT_SIZE)
#define PG_L2_DATA_SIZE (4*PG_DATA_NPAGES)
/* Page Table Info **********************************************************/
/* The number of pages in the in the page table (PG_PGTABLE_NPAGES). We
* position the pagetable PTEs just after the data section PTEs.
*/
#define PG_PGTABLE_NPAGES (PGTABLE_SIZE >> PAGESHIFT)
#define PG_L1_PGTABLE_PADDR (PGTABLE_BASE_PADDR + ((PGTABLE_BASE_VADDR >> 20) << 2))
#define PG_L1_PGTABLE_VADDR (PGTABLE_BASE_VADDR + ((PGTABLE_BASE_VADDR >> 20) << 2))
#define PG_L2_PGTABLE_PADDR (PG_L2_DATA_PADDR + PG_L2_DATA_SIZE)
#define PG_L2_PGTABLE_VADDR (PG_L2_DATA_VADDR + PG_L2_DATA_SIZE)
#define PG_L2_PGTABLE_SIZE (4*PG_DATA_NPAGES)
/* Vector Mapping ***********************************************************/
/* One page is required to map the vector table. The vector table could lie
* at virtual address zero (or at the start of RAM which is aliased to address
* zero on the ea3131) or at virtual address 0xfff00000. We only have logic
* here to support the former case.
*
* NOTE: If the vectors are at address zero, the page table will be
* forced to the highest RAM addresses. If the vectors are at 0xfff0000,
* then the page table is forced to the beginning of RAM.
*
* When the vectors are at the beginning of RAM, they will probably overlap
* the first page of the locked text region. In any other case, the
* configuration must set CONFIG_PAGING_VECPPAGE to provide the physical
* address of the page to use for the vectors.
*
* When the vectors overlap the first page of the locked text region (the
* only case in use so far), then the text page will be temporarily be made
* writable in order to copy the vectors.
*
* PG_VECT_PBASE - This the physical address of the page in memory to be
* mapped to the vector address.
* PG_L2_VECT_PADDR - This is the physical address of the L2 page table
* entry to use for the vector mapping.
* PG_L2_VECT_VADDR - This is the virtual address of the L2 page table
* entry to use for the vector mapping.
*/
/* Case 1: The configuration tells us everything */
#if defined(CONFIG_PAGING_VECPPAGE)
# define PG_VECT_PBASE CONFIG_PAGING_VECPPAGE
# define PG_L2_VECT_PADDR CONFIG_PAGING_VECL2PADDR
# define PG_L2_VECT_VADDR CONFIG_PAGING_VECL2VADDR
/* Case 2: Vectors are in low memory and the locked text region starts at
* the beginning of SRAM (which will be aliased to address 0x00000000).
* However, the beginning of SRAM may not be aligned to the beginning
* of the L2 page table (because the beginning of RAM is offset into
* the table.
*/
#elif defined(CONFIG_ARCH_LOWVECTORS) && !defined(CONFIG_PAGING_LOCKED_PBASE)
# define PG_VECT_PBASE PG_LOCKED_PBASE
# define PG_L2_VECT_OFFSET (((PG_LOCKED_VBASE & 0x000fffff) >> PAGESHIFT) << 2)
# define PG_L2_VECT_PADDR (PGTABLE_L2_BASE_PADDR + PG_L2_VECT_OFFSET)
# define PG_L2_VECT_VADDR (PGTABLE_L2_BASE_VADDR + PG_L2_VECT_OFFSET)
/* Case 3: High vectors or the locked region is not at the beginning or SRAM */
#else
# error "Logic missing for high vectors in this case"
#endif
/* Page Usage ***************************************************************/
/* This is the total number of pages used in the text/data mapping: */
#define PG_TOTAL_NPPAGES (PG_TEXT_NPPAGES + PG_DATA_NPAGES + PG_PGTABLE_NPAGES)
#define PG_TOTAL_NVPAGES (PG_TEXT_NVPAGES + PG_DATA_NPAGES + PG_PGTABLE_NPAGES)
#define PG_TOTAL_PSIZE (PG_TOTAL_NPPAGES << PAGESHIFT)
#define PG_TOTAL_VSIZE (PG_TOTAL_NVPAGES << PAGESHIFT)
/* Sanity check: */
#if PG_TOTAL_NPPAGES > PG_RAM_PAGES
# error "Total pages required exceeds RAM size"
#endif
/* Page Management **********************************************************/
/* For page managment purposes, the following summarize the "heap" of
* free pages, operations on free pages and the L2 page table.
*
* PG_POOL_VA2L1OFFSET(va) - Given a virtual address, return the L1 table
* offset (in bytes).
* PG_POOL_VA2L1VADDR(va) - Given a virtual address, return the virtual
* address of the L1 table entry
* PG_POOL_L12PPTABLE(L1) - Given the value of an L1 table entry return
* the physical address of the start of the L2
* page table
* PG_POOL_L12PPTABLE(L1) - Given the value of an L1 table entry return
* the virtual address of the start of the L2
* page table.
*
* PG_POOL_L1VBASE - The virtual address of the start of the L1
* page table range corresponding to the first
* virtual address of the paged text region.
* PG_POOL_L1VEND - The virtual address of the end+1 of the L1
* page table range corresponding to the last
* virtual address+1 of the paged text region.
*
* PG_POOL_VA2L2NDX(va) - Converts a virtual address within the paged
* text region to the most compact possible
* representation. Each PAGESIZE of address
* corresponds to 1 index in the L2 page table;
* Index 0 corresponds to the first L2 page table
* entry for the first page in the virtual paged
* text address space.
* PG_POOL_NDX2VA(ndx) - Performs the opposite conversion.. convests
* an index into a virtual address in the paged
* text region (the address at the beginning of
* the page).
* PG_POOL_MAXL2NDX - This is the maximum value+1 of such an index.
*
* PG_POOL_PGPADDR(ndx) - Converts an page index into the corresponding
* (physical) address of the backing page memory.
* PG_POOL_PGVADDR(ndx) - Converts an page index into the corresponding
* (virtual)address of the backing page memory.
*
* These are used as follows: If a miss occurs at some virtual address, va,
* A new page index, ndx, is allocated. PG_POOL_PGPADDR(i) converts the index
* into the physical address of the page memory; PG_POOL_L2VADDR(va) converts
* the virtual address in the L2 page table there the new mapping will be
* written.
*/
#define PG_POOL_VA2L1OFFSET(va) (((va) >> 20) << 2)
#define PG_POOL_VA2L1VADDR(va) (PGTABLE_BASE_VADDR + PG_POOL_VA2L1OFFSET(va))
#define PG_POOL_L12PPTABLE(L1) ((L1) & PG_L1_PADDRMASK)
#define PG_POOL_L12VPTABLE(L1) (PG_POOL_L12PPTABLE(L1) - PGTABLE_BASE_PADDR + PGTABLE_BASE_VADDR)
#define PG_POOL_L1VBASE (PGTABLE_BASE_VADDR + ((PG_PAGED_VBASE >> 20) << 2))
#define PG_POOL_L1VEND (PG_POOL_L1VBASE + (CONFIG_PAGING_NVPAGED << 2))
#define PG_POOL_VA2L2NDX(va) (((va) - PG_PAGED_VBASE) >> PAGESHIFT)
#define PG_POOL_NDX2VA(ndx) (((ndx) << PAGESHIFT) + PG_PAGED_VBASE)
#define PG_POOL_MAXL2NDX PG_POOL_VA2L2NDX(PG_PAGED_VEND)
#define PG_POOL_PGPADDR(ndx) (PG_PAGED_PBASE + ((ndx) << PAGESHIFT))
#define PG_POOL_PGVADDR(ndx) (PG_PAGED_VBASE + ((ndx) << PAGESHIFT))
#endif /* CONFIG_PAGING */
/****************************************************************************
* Assembly Macros
****************************************************************************/
#ifdef __ASSEMBLY__
/****************************************************************************
* Name: pg_l2map
*
* Description:
* Write several, contiguous L2 page table entries. npages entries will be
* written. This macro is used when CONFIG_PAGING is enable. This case,
* it is used asfollows:
*
* ldr r0, =PGTABLE_L2_BASE_PADDR <-- Address in L2 table
* ldr r1, =PG_LOCKED_PBASE <-- Physical page memory address
* ldr r2, =CONFIG_PAGING_NLOCKED <-- number of pages
* ldr r3, =MMUFLAGS <-- L2 MMU flags
* pg_l2map r0, r1, r2, r3, r4
*
* Inputs:
* l2 - Physical or virtual start address in the L2 page table, depending
* upon the context. (modified)
* ppage - The physical address of the start of the region to span. Must
* be aligned to 1Mb section boundaries (modified)
* npages - Number of pages to write in the section (modified)
* mmuflags - L2 MMU FLAGS
*
* Scratch registers (modified): tmp
* l2 - Next address in the L2 page table.
* ppage - Start of next physical page
* npages - Loop counter
* tmp - scratch
*
* Assumptions:
* - The MMU is not yet enabled
* - The L2 page tables have been zeroed prior to calling this function
* - pg_l1span has been called to initialize the L1 table.
*
****************************************************************************/
#ifdef CONFIG_PAGING
.macro pg_l2map, l2, ppage, npages, mmuflags, tmp
b 2f
1:
/* Write the one L2 entries. First, get tmp = (ppage | mmuflags),
* the value to write into the L2 PTE
*/
orr \tmp, \ppage, \mmuflags
/* Write value into table at the current table address
* (and increment the L2 page table address by 4)
*/
str \tmp, [\l2], #4
/* Update the physical address that will correspond to the next
* table entry.
*/
add \ppage, \ppage, #CONFIG_PAGING_PAGESIZE
/* Decrement the number of pages written */
sub \npages, \npages, #1
2:
/* Check if all of the pages have been written. If not, then
* loop and write the next PTE.
*/
cmp \npages, #0
bgt 1b
.endm
#endif /* CONFIG_PAGING */
/****************************************************************************
* Name: pg_l1span
*
* Description:
* Write several, contiguous unmapped coarse L1 page table entries. As
* many entries will be written as many as needed to span npages. This
* macro is used when CONFIG_PAGING is enable. This case, it is used as
* follows:
*
* ldr r0, =PG_L1_PGTABLE_PADDR <-- Address in the L1 table
* ldr r1, =PG_L2_PGTABLE_PADDR <-- Physical address of L2 page table
* ldr r2, =PG_PGTABLE_NPAGES <-- Total number of pages
* ldr r3, =PG_PGTABLE_NPAGE1 <-- Number of pages in the first PTE
* ldr r4, =MMU_L1_PGTABFLAGS <-- L1 MMU flags
* pg_l1span r0, r1, r2, r3, r4, r4
*
* Inputs (unmodified unless noted):
* l1 - Physical or virtual address in the L1 table to begin writing (modified)
* l2 - Physical start address in the L2 page table (modified)
* npages - Number of pages to required to span that memory region (modified)
* ppage - The number of pages in page 1 (modified)
* mmuflags - L1 MMU flags to use
*
* Scratch registers (modified): l1, l2, npages, tmp
* l1 - Next L1 table address
* l2 - Physical start address of the next L2 page table
* npages - Loop counter
* ppage - After the first page, this will be the full number of pages.
* tmp - scratch
*
* Return:
* Nothing of interest.
*
* Assumptions:
* - The MMU is not yet enabled
* - The L2 page tables have been zeroed prior to calling this function
*
****************************************************************************/
#ifdef CONFIG_PAGING
.macro pg_l1span, l1, l2, npages, ppage, mmuflags, tmp
b 2f
1:
/* Write the L1 table entry that refers to this (unmapped) coarse page
* table.
*
* tmp = (l2table | mmuflags), the value to write into the page table
*/
orr \tmp, \l2, \mmuflags
/* Write the value into the L1 table at the correct offset.
* (and increment the L1 table address by 4)
*/
str \tmp, [\l1], #4
/* Update the L2 page table address for the next L1 table entry. */
add \l2, \l2, #PT_SIZE /* Next L2 page table start address */
/* Update the number of pages that we have account for (with
* non-mappings). NOTE that the first page may have fewer than
* the maximum entries per page table.
*/
sub \npages, \npages, \ppage
mov \ppage, #PTE_NPAGES
2:
/* Check if all of the pages have been written. If not, then
* loop and write the next L1 entry.
*/
cmp \npages, #0
bgt 1b
.endm
#endif /* CONFIG_PAGING */
#endif /* __ASSEMBLY__ */
/****************************************************************************
* Inline Functions
****************************************************************************/
#ifndef __ASSEMBLY__
#endif /* __ASSEMBLY__ */
#endif /* __ARCH_ARM_SRC_ARM_PG_MACROS_H */
nuttx/arch/arm/src/arm/up_allocpage.c
-243
View File
@@ -1,243 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_allocpage.c
* Allocate a new page and map it to the fault address of a task.
*
* Copyright (C) 2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/sched.h>
#ifdef CONFIG_PAGING
#include <nuttx/page.h>
#include "pg_macros.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Types
****************************************************************************/
#if CONFIG_PAGING_NPPAGED < 256
typedef uint8_t pgndx_t;
#elif CONFIG_PAGING_NPPAGED < 65536
typedef uint16_t pgndx_t;
#else
typedef uint32_t pgndx_t;
#endif
#if PG_POOL_MAXL1NDX < 256
typedef uint8_t L1ndx_t;
#elif PG_POOL_MAXL1NDX < 65536
typedef uint16_t L1ndx_t;
#else
typedef uint32_t L1ndx_t;
#endif
/****************************************************************************
* Private Data
****************************************************************************/
/* Free pages in memory are managed by indices ranging from up to
* CONFIG_PAGING_NPAGED. Initially all pages are free so the page can be
* simply allocated in order: 0, 1, 2, ... . After all CONFIG_PAGING_NPAGED
* pages have be filled, then they are blindly freed and re-used in the
* same order 0, 1, 2, ... because we don't know any better. No smart "least
* recently used" kind of logic is supported.
*/
static pgndx_t g_pgndx;
/* After CONFIG_PAGING_NPAGED have been allocated, the pages will be re-used.
* In order to re-used the page, we will have un-map the page from its previous
* mapping. In order to that, we need to be able to map a physical address to
* to an index into the PTE where it was mapped. The following table supports
* this backward lookup - it is indexed by the page number index, and holds
* another index to the mapped virtual page.
*/
static L1ndx_t g_ptemap[CONFIG_PAGING_NPPAGED];
/* The contents of g_ptemap[] are not valid until g_pgndx has wrapped at
* least one time.
*/
static bool g_pgwrap;
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_allocpage()
*
* Description:
* This architecture-specific function will set aside page in memory and map
* the page to its correct virtual address. Architecture-specific context
* information saved within the TCB will provide the function with the
* information needed to identify the virtual miss address.
*
* This function will return the allocated physical page address in vpage.
* The size of the underlying physical page is determined by the
* configuration setting CONFIG_PAGING_PAGESIZE.
*
* NOTE 1: This function must always return a page allocation. If all
* available pages are in-use (the typical case), then this function will
* select a page in-use, un-map it, and make it available.
*
* NOTE 2: If an in-use page is un-mapped, it may be necessary to flush the
* instruction cache in some architectures.
*
* NOTE 3: Allocating and filling a page is a two step process. up_allocpage()
* allocates the page, and up_fillpage() fills it with data from some non-
* volatile storage device. This distinction is made because up_allocpage()
* can probably be implemented in board-independent logic whereas up_fillpage()
* probably must be implemented as board-specific logic.
*
* NOTE 4: The initial mapping of vpage should be read-able and write-
* able (but not cached). No special actions will be required of
* up_fillpage() in order to write into this allocated page.
*
* Input Parameters:
* tcb - A reference to the task control block of the task that needs to
* have a page fill. Architecture-specific logic can retrieve page
* fault information from the architecture-specific context
* information in this TCB to perform the mapping.
*
* Returned Value:
* This function will return zero (OK) if the allocation was successful.
* A negated errno value may be returned if an error occurs. All errors,
* however, are fatal.
*
* Assumptions:
* - This function is called from the normal tasking context (but with
* interrupts disabled). The implementation must take whatever actions
* are necessary to assure that the operation is safe within this
* context.
*
****************************************************************************/
int up_allocpage(FAR _TCB *tcb, FAR void **vpage)
{
uintptr_t vaddr;
uintptr_t paddr;
uint32_t *pte;
unsigned int pgndx;
/* Since interrupts are disabled, we don't need to anything special. */
DEBUGASSERT(tcb && vpage);
/* Get the virtual address that caused the fault */
vaddr = tcb->xcp.far;
DEBUGASSERT(vaddr >= PG_PAGED_VBASE && vaddr < PG_PAGED_VEND);
/* Allocate page memory to back up the mapping. Start by getting the
* index of the next page that we are going to allocate.
*/
pgndx = g_pgndx++;
if (g_pgndx >= CONFIG_PAGING)
{
g_pgndx = 0;
g_pgwrap = true;
}
/* Was this physical page previously mapped? If so, then we need to un-map
* it.
*/
if (g_pgwrap)
{
/* Yes.. Get a pointer to the L2 entry corresponding to the previous
* mapping -- then zero it!
*/
uintptr_t oldvaddr = PG_POOL_NDX2VA(g_ptemap[pgndx]);
pte = up_va2pte(oldvaddr);
*pte = 0;
/* Invalidate the instruction TLB corresponding to the virtual address */
tlb_inst_invalidate_single(oldvaddr);
/* I do not believe that it is necessary to flush the I-Cache in this
* case: The I-Cache uses a virtual address index and, hence, since the
* NuttX address space is flat, the cached instruction value should be
* correct even if the page mapping is no longer in place.
*/
}
/* Then convert the index to a (physical) page address. */
paddr = PG_POOL_PGPADDR(pgndx);
/* Now setup up the new mapping. Get a pointer to the L2 entry
* corresponding to the new mapping. Then set it map to the newly
* allocated page address. The inital mapping is read/write but
* non-cached (MMU_L2_ALLOCFLAGS)
*/
pte = up_va2pte(vaddr);
*pte = (paddr | MMU_L2_ALLOCFLAGS);
/* And save the new L1 index */
g_ptemap[pgndx] = PG_POOL_VA2L2NDX(vaddr);
/* Finally, return the virtual address of allocated page */
*vpage = (void*)(vaddr & ~PAGEMASK);
return OK;
}
#endif /* CONFIG_PAGING */
nuttx/arch/arm/src/arm/up_assert.c
-325
View File
@@ -1,325 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_assert.c
*
* Copyright (C) 2007-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include <debug.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include <arch/board/board.h>
#include "up_arch.h"
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Output debug info if stack dump is selected -- even if
* debug is not selected.
*/
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
# define lldbg lib_lowprintf
#endif
/* The following is just intended to keep some ugliness out of the mainline
* code. We are going to print the task name if:
*
* CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name
* (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used)
* defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used
*/
#undef CONFIG_PRINT_TASKNAME
#if CONFIG_TASK_NAME_SIZE > 0 && (defined(CONFIG_DEBUG) || defined(CONFIG_ARCH_STACKDUMP))
# define CONFIG_PRINT_TASKNAME 1
#endif
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: up_getsp
****************************************************************************/
/* I don't know if the builtin to get SP is enabled */
static inline uint32_t up_getsp(void)
{
uint32_t sp;
__asm__
(
"\tmov %0, sp\n\t"
: "=r"(sp)
);
return sp;
}
/****************************************************************************
* Name: up_stackdump
****************************************************************************/
#ifdef CONFIG_ARCH_STACKDUMP
static void up_stackdump(uint32_t sp, uint32_t stack_base)
{
uint32_t stack ;
for (stack = sp & ~0x1f; stack < stack_base; stack += 32)
{
uint32_t *ptr = (uint32_t*)stack;
lldbg("%08x: %08x %08x %08x %08x %08x %08x %08x %08x\n",
stack, ptr[0], ptr[1], ptr[2], ptr[3],
ptr[4], ptr[5], ptr[6], ptr[7]);
}
}
#else
# define up_stackdump()
#endif
/****************************************************************************
* Name: up_registerdump
****************************************************************************/
#ifdef CONFIG_ARCH_STACKDUMP
static inline void up_registerdump(void)
{
/* Are user registers available from interrupt processing? */
if (current_regs)
{
int regs;
/* Yes.. dump the interrupt registers */
for (regs = REG_R0; regs <= REG_R15; regs += 8)
{
uint32_t *ptr = (uint32_t*)&current_regs[regs];
lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n",
regs, ptr[0], ptr[1], ptr[2], ptr[3],
ptr[4], ptr[5], ptr[6], ptr[7]);
}
lldbg("CPSR: %08x\n", current_regs[REG_CPSR]);
}
}
#else
# define up_registerdump()
#endif
/****************************************************************************
* Name: up_dumpstate
****************************************************************************/
#ifdef CONFIG_ARCH_STACKDUMP
static void up_dumpstate(void)
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
uint32_t sp = up_getsp();
uint32_t ustackbase;
uint32_t ustacksize;
#if CONFIG_ARCH_INTERRUPTSTACK > 3
uint32_t istackbase;
uint32_t istacksize;
#endif
/* Get the limits on the user stack memory */
if (rtcb->pid == 0)
{
ustackbase = g_heapbase - 4;
ustacksize = CONFIG_IDLETHREAD_STACKSIZE;
}
else
{
ustackbase = (uint32_t)rtcb->adj_stack_ptr;
ustacksize = (uint32_t)rtcb->adj_stack_size;
}
/* Get the limits on the interrupt stack memory */
#if CONFIG_ARCH_INTERRUPTSTACK > 3
istackbase = (uint32_t)&g_userstack;
istacksize = (CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4;
/* Show interrupt stack info */
lldbg("sp: %08x\n", sp);
lldbg("IRQ stack:\n");
lldbg(" base: %08x\n", istackbase);
lldbg(" size: %08x\n", istacksize);
/* Does the current stack pointer lie within the interrupt
* stack?
*/
if (sp <= istackbase && sp > istackbase - istacksize)
{
/* Yes.. dump the interrupt stack */
up_stackdump(sp, istackbase);
/* Extract the user stack pointer which should lie
* at the base of the interrupt stack.
*/
sp = g_userstack;
lldbg("sp: %08x\n", sp);
}
/* Show user stack info */
lldbg("User stack:\n");
lldbg(" base: %08x\n", ustackbase);
lldbg(" size: %08x\n", ustacksize);
#else
lldbg("sp: %08x\n", sp);
lldbg("stack base: %08x\n", ustackbase);
lldbg("stack size: %08x\n", ustacksize);
#endif
/* Dump the user stack if the stack pointer lies within the allocated user
* stack memory.
*/
if (sp > ustackbase || sp <= ustackbase - ustacksize)
{
#if !defined(CONFIG_ARCH_INTERRUPTSTACK) || CONFIG_ARCH_INTERRUPTSTACK < 4
lldbg("ERROR: Stack pointer is not within allocated stack\n");
#endif
}
else
{
up_stackdump(sp, ustackbase);
}
/* Then dump the registers (if available) */
up_registerdump();
}
#else
# define up_dumpstate()
#endif
/****************************************************************************
* Name: _up_assert
****************************************************************************/
static void _up_assert(int errorcode) /* __attribute__ ((noreturn)) */
{
/* Are we in an interrupt handler or the idle task? */
if (current_regs || ((_TCB*)g_readytorun.head)->pid == 0)
{
(void)irqsave();
for(;;)
{
#ifdef CONFIG_ARCH_LEDS
up_ledon(LED_PANIC);
up_mdelay(250);
up_ledoff(LED_PANIC);
up_mdelay(250);
#endif
}
}
else
{
exit(errorcode);
}
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_assert
****************************************************************************/
void up_assert(const uint8_t *filename, int lineno)
{
#ifdef CONFIG_PRINT_TASKNAME
_TCB *rtcb = (_TCB*)g_readytorun.head;
#endif
up_ledon(LED_ASSERTION);
#ifdef CONFIG_PRINT_TASKNAME
lldbg("Assertion failed at file:%s line: %d task: %s\n",
filename, lineno, rtcb->name);
#else
lldbg("Assertion failed at file:%s line: %d\n",
filename, lineno);
#endif
up_dumpstate();
_up_assert(EXIT_FAILURE);
}
/****************************************************************************
* Name: up_assert_code
****************************************************************************/
void up_assert_code(const uint8_t *filename, int lineno, int errorcode)
{
#ifdef CONFIG_PRINT_TASKNAME
_TCB *rtcb = (_TCB*)g_readytorun.head;
#endif
up_ledon(LED_ASSERTION);
#ifdef CONFIG_PRINT_TASKNAME
lldbg("Assertion failed at file:%s line: %d task: %s error code: %d\n",
filename, lineno, rtcb->name, errorcode);
#else
lldbg("Assertion failed at file:%s line: %d error code: %d\n",
filename, lineno, errorcode);
#endif
up_dumpstate();
_up_assert(errorcode);
}
nuttx/arch/arm/src/arm/up_blocktask.c
-167
View File
@@ -1,167 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_blocktask.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdbool.h>
#include <sched.h>
#include <debug.h>
#include <nuttx/arch.h>
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_block_task
*
* Description:
* The currently executing task at the head of
* the ready to run list must be stopped. Save its context
* and move it to the inactive list specified by task_state.
*
* Inputs:
* tcb: Refers to a task in the ready-to-run list (normally
* the task at the head of the list). It most be
* stopped, its context saved and moved into one of the
* waiting task lists. It it was the task at the head
* of the ready-to-run list, then a context to the new
* ready to run task must be performed.
* task_state: Specifies which waiting task list should be
* hold the blocked task TCB.
*
****************************************************************************/
void up_block_task(_TCB *tcb, tstate_t task_state)
{
/* Verify that the context switch can be performed */
if ((tcb->task_state < FIRST_READY_TO_RUN_STATE) ||
(tcb->task_state > LAST_READY_TO_RUN_STATE))
{
PANIC(OSERR_BADBLOCKSTATE);
}
else
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
bool switch_needed;
/* Remove the tcb task from the ready-to-run list. If we
* are blocking the task at the head of the task list (the
* most likely case), then a context switch to the next
* ready-to-run task is needed. In this case, it should
* also be true that rtcb == tcb.
*/
switch_needed = sched_removereadytorun(tcb);
/* Add the task to the specified blocked task list */
sched_addblocked(tcb, (tstate_t)task_state);
/* If there are any pending tasks, then add them to the g_readytorun
* task list now
*/
if (g_pendingtasks.head)
{
switch_needed |= sched_mergepending();
}
/* Now, perform the context switch if one is needed */
if (switch_needed)
{
/* Are we in an interrupt handler? */
if (current_regs)
{
/* Yes, then we have to do things differently.
* Just copy the current_regs into the OLD rtcb.
*/
up_savestate(rtcb->xcp.regs);
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
/* Then switch contexts */
up_restorestate(rtcb->xcp.regs);
}
/* Copy the user C context into the TCB at the (old) head of the
* g_readytorun Task list. if up_saveusercontext returns a non-zero
* value, then this is really the previously running task restarting!
*/
else if (!up_saveusercontext(rtcb->xcp.regs))
{
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
/* Then switch contexts */
up_fullcontextrestore(rtcb->xcp.regs);
}
}
}
}
nuttx/arch/arm/src/arm/up_cache.S
-74
View File
@@ -1,74 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_cache.S
*
* Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include "up_internal.h"
#include "up_arch.h"
/****************************************************************************
* Definitions
****************************************************************************/
#define CACHE_DLINESIZE 32
/****************************************************************************
* Assembly Macros
****************************************************************************/
/****************************************************************************
* Name: up_flushicache
****************************************************************************/
/* Esure coherency between the Icache and the Dcache in the region described
* by r0=start and r1=end.
*/
.globl up_flushicache
.type up_flushicache,%function
up_flushicache:
bic r0, r0, #CACHE_DLINESIZE - 1
1: mcr p15, 0, r0, c7, c10, 1 /* Clean D entry */
mcr p15, 0, r0, c7, c5, 1 /* Invalidate I entry */
add r0, r0, #CACHE_DLINESIZE
cmp r0, r1
blo 1b
mcr p15, 0, r0, c7, c10, 4 /* Drain WB */
mov pc, lr
.size up_flushicache, .-up_flushicache
.end
nuttx/arch/arm/src/arm/up_checkmapping.c
-123
View File
@@ -1,123 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_checkmapping.c
* Check if the current task's fault address has been mapped into the virtual
* address space.
*
* Copyright (C) 2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <debug.h>
#include <nuttx/sched.h>
#include <nuttx/page.h>
#include "up_internal.h"
#ifdef CONFIG_PAGING
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_checkmapping()
*
* Description:
* The function up_checkmapping() returns an indication if the page fill
* still needs to performed or not. In certain conditions, the page fault
* may occur on several threads and be queued multiple times. This function
* will prevent the same page from be filled multiple times.
*
* Input Parameters:
* tcb - A reference to the task control block of the task that we believe
* needs to have a page fill. Architecture-specific logic can
* retrieve page fault information from the architecture-specific
* context information in this TCB and can consult processor resources
* (page tables or TLBs or ???) to determine if the fill still needs
* to be performed or not.
*
* Returned Value:
* This function will return true if the mapping is in place and false
* if the mapping is still needed. Errors encountered should be
* interpreted as fatal.
*
* Assumptions:
* - This function is called from the normal tasking context (but with
* interrupts disabled). The implementation must take whatever actions
* are necessary to assure that the operation is safe within this
* context.
*
****************************************************************************/
bool up_checkmapping(FAR _TCB *tcb)
{
uintptr_t vaddr;
uint32_t *pte;
/* Since interrupts are disabled, we don't need to anything special. */
DEBUGASSERT(tcb);
/* Get the virtual address that caused the fault */
vaddr = tcb->xcp.far;
DEBUGASSERT(vaddr >= PG_PAGED_VBASE && vaddr < PG_PAGED_VEND);
/* Get the PTE associated with this virtual address */
pte = up_va2pte(vaddr);
/* Return true if this virtual address is mapped. */
return (*pte != 0);
}
#endif /* CONFIG_PAGING */
nuttx/arch/arm/src/arm/up_copystate.c
-82
View File
@@ -1,82 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_copystate.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_undefinedinsn
****************************************************************************/
/* A little faster than most memcpy's */
void up_copystate(uint32_t *dest, uint32_t *src)
{
int i;
/* In the current ARM model, the state is always copied to and from the
* stack and TCB.
*/
for (i = 0; i < XCPTCONTEXT_REGS; i++)
{
*dest++ = *src++;
}
}
nuttx/arch/arm/src/arm/up_dataabort.c
-201
View File
@@ -1,201 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_dataabort.c
*
* Copyright (C) 2007-2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <debug.h>
#include <nuttx/irq.h>
#include "os_internal.h"
#include "up_internal.h"
#ifdef CONFIG_PAGING
# include <nuttx/page.h>
# include "arm.h"
#endif
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Output debug info if stack dump is selected -- even if
* debug is not selected.
*/
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
# define lldbg lib_lowprintf
#endif
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_dataabort
*
* Input parameters:
* regs - The standard, ARM register save array.
*
* If CONFIG_PAGING is selected in the NuttX configuration file, then these
* additional input values are expected:
*
* far - Fault address register. On a data abort, the ARM MMU places the
* miss virtual address (MVA) into the FAR register. This is the address
* of the data which, when accessed, caused the fault.
* fsr - Fault status register. On a data a abort, the ARM MMU places an
* encoded four-bit value, the fault status, along with the four-bit
* encoded domain number, in the data FSR
*
* Description:
* This is the data abort exception handler. The ARM data abort exception
* occurs when a memory fault is detected during a data transfer.
*
****************************************************************************/
#ifdef CONFIG_PAGING
void up_dataabort(uint32_t *regs, uint32_t far, uint32_t fsr)
{
FAR _TCB *tcb = (FAR _TCB *)g_readytorun.head;
#ifdef CONFIG_PAGING
uint32_t *savestate;
/* Save the saved processor context in current_regs where it can be accessed
* for register dumps and possibly context switching.
*/
savestate = (uint32_t*)current_regs;
#endif
current_regs = regs;
#ifdef CONFIG_PAGING
/* In the NuttX on-demand paging implementation, only the read-only, .text
* section is paged. However, the ARM compiler generated PC-relative data
* fetches from within the .text sections. Also, it is customary to locate
* read-only data (.rodata) within the same section as .text so that it
* does not require copying to RAM. Misses in either of these case should
* cause a data abort.
*
* We are only interested in data aborts due to page translations faults.
* Sections should already be in place and permissions should already be
* be set correctly (to read-only) so any other data abort reason is a
* fatal error.
*/
pglldbg("FSR: %08x FAR: %08x\n", fsr, far);
if ((fsr & FSR_MASK) != FSR_PAGE)
{
goto segfault;
}
/* Check the (virtual) address of data that caused the data abort. When
* the exception occurred, this address was provided in the FAR register.
* (It has not yet been saved in the register context save area).
*/
pgllvdbg("VBASE: %08x VEND: %08x\n", PG_PAGED_VBASE, PG_PAGED_VEND);
if (far < PG_PAGED_VBASE || far >= PG_PAGED_VEND)
{
goto segfault;
}
/* Save the offending data address as the fault address in the TCB of
* the currently task. This fault address is also used by the prefetch
* abort handling; this will allow common paging logic for both
* prefetch and data aborts.
*/
tcb->xcp.far = regs[REG_R15];
/* Call pg_miss() to schedule the page fill. A consequences of this
* call are:
*
* (1) The currently executing task will be blocked and saved on
* on the g_waitingforfill task list.
* (2) An interrupt-level context switch will occur so that when
* this function returns, it will return to a different task,
* most likely the page fill worker thread.
* (3) The page fill worker task has been signalled and should
* execute immediately when we return from this exception.
*/
pg_miss();
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
return;
segfault:
#endif
lldbg("Data abort. PC: %08x FAR: %08x FSR: %08x\n", regs[REG_PC], far, fsr);
PANIC(OSERR_ERREXCEPTION);
}
#else /* CONFIG_PAGING */
void up_dataabort(uint32_t *regs)
{
/* Save the saved processor context in current_regs where it can be accessed
* for register dumps and possibly context switching.
*/
current_regs = regs;
/* Crash -- possibly showing diagnost debug information. */
lldbg("Data abort. PC: %08x\n", regs[REG_PC]);
PANIC(OSERR_ERREXCEPTION);
}
#endif /* CONFIG_PAGING */
nuttx/arch/arm/src/arm/up_doirq.c
-114
View File
@@ -1,114 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_doirq.c
*
* Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include <assert.h>
#include <arch/board/board.h>
#include "up_arch.h"
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
void up_doirq(int irq, uint32_t *regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
PANIC(OSERR_ERREXCEPTION);
#else
uint32_t *savestate;
/* Nested interrupts are not supported in this implementation. If you want
* implemented nested interrupts, you would have to (1) change the way that
* current regs is handled and (2) the design associated with
* CONFIG_ARCH_INTERRUPTSTACK.
*/
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Mask and acknowledge the interrupt */
up_maskack_irq(irq);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still disabled) */
up_enable_irq(irq);
#endif
up_ledoff(LED_INIRQ);
}
nuttx/arch/arm/src/arm/up_fullcontextrestore.S
-118
View File
@@ -1,118 +0,0 @@
/**************************************************************************
* arch/arm/src/arm/up_fullcontextrestore.S
*
* Copyright (C) 2007, 2009-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/irq.h>
#include "up_internal.h"
/**************************************************************************
* Private Definitions
**************************************************************************/
/**************************************************************************
* Private Types
**************************************************************************/
/**************************************************************************
* Private Function Prototypes
**************************************************************************/
/**************************************************************************
* Global Variables
**************************************************************************/
/**************************************************************************
* Private Variables
**************************************************************************/
/**************************************************************************
* Private Functions
**************************************************************************/
/**************************************************************************
* Public Functions
**************************************************************************/
/**************************************************************************
* Name: up_fullcontextrestore
**************************************************************************/
.globl up_fullcontextrestore
.type up_fullcontextrestore, function
up_fullcontextrestore:
/* On entry, a1 (r0) holds address of the register save area */
/* Recover all registers except for r0, r1, R15, and CPSR */
add r1, r0, #(4*REG_R2) /* Offset to REG_R2 storage */
ldmia r1, {r2-r14} /* Recover registers */
/* Create a stack frame to hold the PC */
sub sp, sp, #(3*4) /* Frame for three registers */
ldr r1, [r0, #(4*REG_R0)] /* Fetch the stored r0 value */
str r1, [sp] /* Save it at the top of the stack */
ldr r1, [r0, #(4*REG_R1)] /* Fetch the stored r1 value */
str r1, [sp, #4] /* Save it in the stack */
ldr r1, [r0, #(4*REG_PC)] /* Fetch the stored pc value */
str r1, [sp, #8] /* Save it at the bottom of the frame */
/* Now we can restore the CPSR. We wait until we are completely
* finished with the context save data to do this. Restore the CPSR
* may re-enable and interrupts and we could be in a context
* where the save structure is only protected by interrupts being
* disabled.
*/
ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the stored CPSR value */
msr cpsr, r1 /* Set the CPSR */
/* Now recover r0 and r1 */
ldr r0, [sp]
ldr r1, [sp, #4]
add sp, sp, #(2*4)
/* Then return to the address at the stop of the stack,
* destroying the stack frame
*/
ldr pc, [sp], #4
.size up_fullcontextrestore, . - up_fullcontextrestore
nuttx/arch/arm/src/arm/up_head.S
-638
View File
@@ -1,638 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_head.S
*
* Copyright (C) 2007, 2009-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include "arm.h"
#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"
#ifdef CONFIG_PAGING
# include <nuttx/page.h>
# include "pg_macros.h"
#endif
/**********************************************************************************
* Configuration
**********************************************************************************/
#undef ALIGNMENT_TRAP
#undef CPU_DCACHE_WRITETHROUGH
#undef CPU_CACHE_ROUND_ROBIN
#undef CPU_DCACHE_DISABLE
#undef CPU_ICACHE_DISABLE
/* There are three operational memory configurations:
*
* 1. We execute in place in FLASH (CONFIG_BOOT_RUNFROMFLASH=y). In this case
* the boot logic must:
*
* - Configure SDRAM,
* - Initialize the .data section in RAM, and
* - Clear .bss section
*/
#ifdef CONFIG_BOOT_RUNFROMFLASH
# error "Configuration not implemented"
# define CONFIG_SDRAM 1
/* Check for the identity mapping: For this configuration, this would be
* the case where the virtual beginning of FLASH is the same as the physical
* beginning of FLASH.
*/
# if CONFIG_FLASH_START == CONFIG_FLASH_VSTART
# define CONFIG_IDENTITY_TEXTMAP 1
# endif
/* 2. We boot in FLASH but copy ourselves to DRAM from better performance.
* (CONFIG_BOOT_RUNFROMFLASH=n && CONFIG_BOOT_COPYTORAM=y). In this case
* the boot logic must:
*
* - Configure SDRAM,
* - Copy ourself to DRAM (after mapping it), and
* - Clear .bss section
*
* In this case, we assume that the logic within this file executes from FLASH.
*/
#elif defined(CONFIG_BOOT_COPYTORAM)
# error "configuration not implemented
# define CONFIG_SDRAM 1
/* Check for the identity mapping: For this configuration, this would be
* the case where the virtual beginning of FLASH is the same as the physical
* beginning of FLASH.
*/
# if CONFIG_FLASH_START == CONFIG_FLASH_VSTART
# define CONFIG_IDENTITY_TEXTMAP 1
# endif
/* 3. There is bootloader that copies us to DRAM (but probably not to the beginning)
* (CONFIG_BOOT_RUNFROMFLASH=n && CONFIG_BOOT_COPYTORAM=n). In this case SDRAM
* was initialized by the boot loader, and this boot logic must:
*
* - Clear .bss section
*/
#else
/* Check for the identity mapping: For this configuration, this would be
* the case where the virtual beginning of RAM is the same as the physical
* beginning of RAM.
*/
# if CONFIG_DRAM_START == CONFIG_DRAM_VSTART
# define CONFIG_IDENTITY_TEXTMAP 1
# endif
#endif
/* For each page table offset, the following provide (1) the physical address of
* the start of the page table and (2) the number of page table entries in the
* first page table.
*
* Coarse: PG_L1_PADDRMASK=0xfffffc00
* NPAGE1=(256 -((a) & 0x000003ff) >> 2) NPAGE1=1-256
* Fine: PG_L1_PADDRMASK=0xfffff000
* NPAGE1=(1024 -((a) & 0x00000fff) >> 2) NPAGE1=1-1024
*/
#ifdef CONFIG_PAGING
# define PG_L2_TEXT_PBASE (PG_L2_TEXT_PADDR & PG_L1_PADDRMASK)
# define PG_L2_TEXT_NPAGE1 (PTE_NPAGES - ((PG_L2_TEXT_PADDR & ~PG_L1_PADDRMASK) >> 2))
# define PG_L2_PGTABLE_PBASE (PG_L2_PGTABLE_PADDR & PG_L1_PADDRMASK)
# define PG_L2_PGTABLE_NPAGE1 (PTE_NPAGES - ((PG_L2_PGTABLE_PADDR & ~PG_L1_PADDRMASK) >> 2))
# define PG_L2_DATA_PBASE (PG_L2_DATA_PADDR & PG_L1_PADDRMASK)
# define PG_L2_DATA_NPAGE1 (PTE_NPAGES - ((PG_L2_DATA_PADDR & ~PG_L1_PADDRMASK) >> 2))
#endif
/****************************************************************************
* Definitions
****************************************************************************/
/* RX_NSECTIONS determines the number of 1Mb sections to map for the
* Read/eXecute address region. This is based on CONFIG_DRAM_SIZE. For most
* ARM9 architectures, CONFIG_DRAM_SIZE describes the size of installed SDRAM.
* But for other architectures, this might refer to the size of FLASH or
* SRAM regions. (bad choice of naming).
*/
#define RX_NSECTIONS ((CONFIG_DRAM_SIZE+0x000fffff) >> 20)
/****************************************************************************
* Assembly Macros
****************************************************************************/
/* The ARM9 L1 page table can be placed at the beginning or at the end of the
* RAM space. This decision is based on the placement of the vector area:
* If the vectors are place in low memory at address 0x0000 0000, then the
* page table is placed in high memory; if the vectors are placed in high
* memory at address 0xfff0 0000, then the page table is locating at the
* beginning of RAM.
*
* For the special case where (1) the program executes out of RAM, and (2) the
* page is located at the beginning of RAM, then the following macro can
* easily find the physical address of the section that includes the first
* part of the text region: Since the page table is closely related to the
* NuttX base address in this case, we can convert the page table base address
* to the base address of the section containing both.
*/
#ifdef CONFIG_ARCH_LOWVECTORS
.macro mksection, section, pgtable
bic \section, \pgtable, #0x000ff000
.endm
#endif
/* This macro will modify r0, r1, r2 and r14 */
#ifdef CONFIG_DEBUG
.macro showprogress, code
mov r0, #\code
bl up_lowputc
.endm
#else
.macro showprogress, code
.endm
#endif
/****************************************************************************
* Name: __start
****************************************************************************/
/* We assume the bootloader has already initialized most of the h/w for us
* and that only leaves us having to do some os specific things below.
*/
.text
.global __start
.type __start, #function
__start:
/* Make sure that we are in SVC mode with all IRQs disabled */
mov r0, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
msr cpsr_c, r0
/* Initialize DRAM using a macro provided by board-specific logic */
#ifdef CONFIG_SDRAM
config_sdram
#endif
/* Clear the 16K level 1 page table */
ldr r4, .LCppgtable /* r4=phys. page table */
#ifndef CONFIG_ARCH_ROMPGTABLE
mov r0, r4
mov r1, #0
add r2, r0, #PGTABLE_SIZE
.Lpgtableclear:
str r1, [r0], #4
str r1, [r0], #4
str r1, [r0], #4
str r1, [r0], #4
teq r0, r2
bne .Lpgtableclear
/* Create identity mapping for first MB of the .text section to support
* this startup logic executing out of the physical address space. This
* identity mapping will be removed by .Lvstart (see below). Of course,
* we would only do this if the physical-virtual mapping is not already
* the identity mapping.
*/
#ifndef CONFIG_IDENTITY_TEXTMAP
mksection r0, r4 /* r0=phys. base section */
ldr r1, .LCmmuflags /* FLGS=MMU_MEMFLAGS */
add r3, r1, r0 /* r3=flags + base */
str r3, [r4, r0, lsr #18] /* identity mapping */
#endif
#ifdef CONFIG_PAGING
/* Map the read-only .text region in place. This must be done
* before the MMU is enabled and the virtual addressing takes
* effect. First populate the L1 table for the locked and paged
* text regions.
*
* We could probably make the the pg_l1span and pg_l2map macros into
* call-able subroutines, but we would have to be carefully during
* this phase while we are operating in a physical address space.
*
* NOTE: That the value of r5 (L1 table base address) must be
* preserved through the following.
*/
adr r0, .Ltxtspan
ldmia r0, {r0, r1, r2, r3, r5}
pg_l1span r0, r1, r2, r3, r5, r6
/* Then populate the L2 table for the locked text region only. */
adr r0, .Ltxtmap
ldmia r0, {r0, r1, r2, r3}
pg_l2map r0, r1, r2, r3, r5
/* Make sure that the page table is itself mapped and and read/write-able.
* First, populate the L1 table:
*/
adr r0, .Lptabspan
ldmia r0, {r0, r1, r2, r3, r5}
pg_l1span r0, r1, r2, r3, r5, r6
/* Then populate the L2 table. */
adr r0, .Lptabmap
ldmia r0, {r0, r1, r2, r3}
pg_l2map r0, r1, r2, r3, r5
#else /* CONFIG_PAGING */
/* Create a virtual single section mapping for the first MB of the .text
* address space. Now, we have the first 1MB mapping to both phyical and
* virtual addresses. The rest of the .text mapping will be completed in
* .Lvstart once we have moved the physical mapping out of the way.
*
* Here we expect to have:
* r4 = Address of the base of the L1 table
*/
ldr r2, .LCvpgtable /* r2=virt. page table */
mksection r0, r2 /* r0=virt. base section */
str r3, [r4, r0, lsr #18] /* identity mapping */
/* NOTE: No .data/.bss access should be attempted. This temporary mapping
* can only be assumed to cover the initial .text region.
*/
#endif /* CONFIG_PAGING */
#endif /* CONFIG_ARCH_ROMPGTABLE */
/* The following logic will set up the ARM920/ARM926 for normal operation.
*
* Here we expect to have:
* r4 = Address of the base of the L1 table
*/
mov r0, #0
mcr p15, 0, r0, c7, c7 /* Invalidate I,D caches */
mcr p15, 0, r0, c7, c10, 4 /* Drain write buffer */
mcr p15, 0, r0, c8, c7 /* Invalidate I,D TLBs */
mcr p15, 0, r4, c2, c0 /* Load page table pointer */
#ifdef CPU_DCACHE_WRITETHROUGH
mov r0, #4 /* Disable write-back on caches explicitly */
mcr p15, 7, r0, c15, c0, 0
#endif
/* Enable the MMU and caches
* lr = Resume at .Lvstart with the MMU enabled
*/
ldr lr, .LCvstart /* Abs. virtual address */
mov r0, #0x1f /* Domains 0, 1 = client */
mcr p15, 0, r0, c3, c0 /* Load domain access register */
mrc p15, 0, r0, c1, c0 /* Get control register */
/* Clear bits (see arm.h)
*
* CR_R - ROM MMU protection
* CR_F - Implementation defined
* CR_Z - Implementation defined
*
* CR_A - Alignment abort enable
* CR_C - Dcache enable
* CR_W - Write buffer enable
*
* CR_I - Icache enable
*/
bic r0, r0, #(CR_R|CR_F|CR_Z)
bic r0, r0, #(CR_A|CR_C|CR_W)
bic r0, r0, #(CR_I)
/* Set bits (see arm.h)
*
* CR_M - MMU enable
* CR_P - 32-bit exception handler
* CR_D - 32-bit data address range
*/
orr r0, r0, #(CR_M|CR_P|CR_D)
/* In most architectures, vectors are relocated to 0xffff0000.
* -- but not all
*
* CR_S - System MMU protection
* CR_V - Vectors relocated to 0xffff0000
*/
#ifndef CONFIG_ARCH_LOWVECTORS
orr r0, r0, #(CR_S|CR_V)
#else
orr r0, r0, #(CR_S)
#endif
/* CR_RR - Round Robin cache replacement */
#ifdef CPU_CACHE_ROUND_ROBIN
orr r0, r0, #(CR_RR)
#endif
/* CR_C - Dcache enable */
#ifndef CPU_DCACHE_DISABLE
orr r0, r0, #(CR_C)
#endif
/* CR_C - Dcache enable */
#ifndef CPU_ICACHE_DISABLE
orr r0, r0, #(CR_I)
#endif
/* CR_A - Alignment abort enable */
#ifdef ALIGNMENT_TRAP
orr r0, r0, #(CR_A)
#endif
mcr p15, 0, r0, c1, c0, 0 /* write control reg */
/* Get TMP=2 Processor ID register */
mrc p15, 0, r1, c0, c0, 0 /* read id reg */
mov r1,r1 /* Null-avoiding nop */
mov r1,r1 /* Null-avoiding nop */
/* And "jump" to .Lvstart */
mov pc, lr
/****************************************************************************
* PC_Relative Data
****************************************************************************/
/* Most addresses are all virtual address */
.type .LCvstart, %object
.LCvstart:
.long .Lvstart
#ifndef CONFIG_ARCH_ROMPGTABLE
.type .LCmmuflags, %object
.LCmmuflags:
.long MMU_MEMFLAGS /* MMU flags for memory sections */
#endif
.type .LCppgtable, %object
.LCppgtable:
.long PGTABLE_BASE_PADDR /* Physical start of page table */
#ifndef CONFIG_ARCH_ROMPGTABLE
.type .LCvpgtable, %object
.LCvpgtable:
.long PGTABLE_BASE_VADDR /* Virtual start of page table */
#endif
#ifdef CONFIG_PAGING
.Ltxtspan:
.long PG_L1_TEXT_PADDR /* Physical address in the L1 table */
.long PG_L2_TEXT_PBASE /* Physical address of the start of the L2 page table */
.long PG_TEXT_NVPAGES /* Total (virtual) text pages to be mapped */
.long PG_L2_TEXT_NPAGE1 /* The number of text pages in the first page table */
.long MMU_L1_TEXTFLAGS /* L1 MMU flags to use */
.Ltxtmap:
.long PG_L2_LOCKED_PADDR /* Physical address in the L2 table */
.long PG_LOCKED_PBASE /* Physical address of locked base memory */
.long CONFIG_PAGING_NLOCKED /* Number of pages in the locked region */
.long MMU_L2_TEXTFLAGS /* L2 MMU flags to use */
.Lptabspan:
.long PG_L1_PGTABLE_PADDR /* Physical address in the L1 table */
.long PG_L2_PGTABLE_PBASE /* Physical address of the start of the L2 page table */
.long PG_PGTABLE_NPAGES /* Total mapped page table pages */
.long PG_L2_PGTABLE_NPAGE1 /* The number of text pages in the first page table */
.long MMU_L1_PGTABFLAGS /* L1 MMU flags to use */
.Lptabmap:
.long PG_L2_PGTABLE_PADDR /* Physical address in the L2 table */
.long PGTABLE_BASE_PADDR /* Physical address of the page table memory */
.long PG_PGTABLE_NPAGES /* Total mapped page table pages */
.long MMU_L2_PGTABFLAGS /* L2 MMU flags to use */
#endif /* CONFIG_PAGING */
.size __start, .-__start
/****************************************************************************
* Name: .Lvstart
***************************************************************************/
/* The following is executed after the MMU has been enabled. This uses
* absolute addresses; this is not position independent.
*/
.align 5
.local .Lvstart
.type .Lvstart, %function
.Lvstart:
/* Remove the temporary mapping (if one was made). The following assumes
* that the total RAM size is > 1Mb and extends that initial mapping to
* cover additinal RAM sections.
*/
#ifndef CONFIG_ARCH_ROMPGTABLE
#ifndef CONFIG_IDENTITY_TEXTMAP
ldr r4, .LCvpgtable /* r4=virtual page table */
ldr r1, .LCppgtable /* r1=phys. page table */
mksection r3, r1 /* r2=phys. base addr */
mov r0, #0 /* flags + base = 0 */
str r0, [r4, r3, lsr #18] /* Undo identity mapping */
#endif
#if defined(CONFIG_PAGING)
/* Populate the L1 table for the data region */
adr r0, .Ldataspan
ldmia r0, {r0, r1, r2, r3, r4}
pg_l1span r0, r1, r2, r3, r4, r5
/* Populate the L2 table for the data region */
adr r0, .Ldatamap
ldmia r0, {r0, r1, r2, r3}
pg_l2map r0, r1, r2, r3, r4
#elif defined(CONFIG_BOOT_RUNFROMFLASH)
# error "Logic not implemented"
#else
/* Now setup the pagetables for our normal SDRAM mappings mapped region.
* We round NUTTX_START_VADDR down to the nearest megabyte boundary.
*/
ldr r1, .LCmmuflags /* FLGS=MMU_MEMFLAGS */
add r3, r3, r1 /* r3=flags + base */
add r0, r4, #(NUTTX_START_VADDR & 0xff000000) >> 18
bic r2, r3, #0x00f00000
str r2, [r0]
add r0, r0, #(NUTTX_START_VADDR & 0x00f00000) >> 18
str r3, [r0], #4
/* Now map the remaining RX_NSECTIONS-1 sections of the executable
* memory region.
*/
.rept RX_NSECTIONS-1
add r3, r3, #SECTION_SIZE
str r3, [r0], #4
.endr
/* If we are executing from RAM with a fixed page configuration, then
* we can assume that the above contiguous mapping included all of the
* .text, .data, .bss, heap, etc. But if we are executing from FLASH,
* then the RAM area is probably in a separate physical address region
* and will require a separate mapping. Or, if we are supporting on-demand
* paging of the .text region, then the RAM-based .data/.bss/heap section
* will still probably be located in a separate (virtual) address region.
*/
#endif /* CONFIG_PAGING */
#endif /* CONFIG_ARCH_ROMPGTABLE */
/* Zero BSS and set up the stack pointer */
adr r0, .Linitparms
ldmia r0, {r0, r1, sp}
/* Clear the frame pointer and .bss */
mov fp, #0
.Lbssinit:
cmp r0, r1 /* Clear up to _bss_end_ */
strcc fp, [r0],#4
bcc .Lbssinit
/* If the .data section is in a separate, unitialized address space,
* then we will also need to copy the initial values of of the .data
* section from the .text region into that .data region. This would
* be the case if we are executing from FLASH and the .data section
* lies in a different physical address region OR if we are support
* on-demand paging and the .data section lies in a different virtual
* address region.
*/
#if defined(CONFIG_BOOT_RUNFROMFLASH) || defined(CONFIG_PAGING)
adr r3, .Ldatainit
ldmia r3, {r0, r1, r2}
1: ldmia r0!, {r3 - r10}
stmia r1!, {r3 - r10}
cmp r1, r2
blt 1b
#endif
/* Perform early C-level, platform-specific initialization */
bl up_boot
/* Finally branch to the OS entry point */
mov lr, #0
b os_start
/* Text-section constants:
*
* _sbss is the start of the BSS region (see ld.script)
* _ebss is the end of the BSS regsion (see ld.script)
*
* The idle task stack starts at the end of BSS and is of size
* CONFIG_IDLETHREAD_STACKSIZE. The heap continues from there until the
* end of memory. See g_heapbase below.
*/
.Linitparms:
.long _sbss
.long _ebss
.long _ebss+CONFIG_IDLETHREAD_STACKSIZE-4
#ifdef CONFIG_PAGING
.Ldataspan:
.long PG_L1_DATA_VADDR /* Virtual address in the L1 table */
.long PG_L2_DATA_PBASE /* Physical address of the start of the L2 page table */
.long PG_DATA_NPAGES /* Number of pages in the data region */
.long PG_L2_DATA_NPAGE1 /* The number of text pages in the first page table */
.long MMU_L1_DATAFLAGS /* L1 MMU flags to use */
.Ldatamap:
.long PG_L2_DATA_VADDR /* Virtual address in the L2 table */
.long PG_DATA_PBASE /* Physical address of data memory */
.long PG_DATA_NPAGES /* Number of pages in the data region */
.long MMU_L2_DATAFLAGS /* L2 MMU flags to use */
#endif /* CONFIG_PAGING */
#if defined(CONFIG_BOOT_RUNFROMFLASH) || defined(CONFIG_PAGING)
.Ldatainit:
.long _eronly /* Where .data defaults are stored in FLASH */
.long _sdata /* Where .data needs to reside in SDRAM */
.long _edata
#endif
.size .Lvstart, .-.Lvstart
/* Data section variables */
/* This global variable is unsigned long g_heapbase and is
* exported from here only because of its coupling to .Linitparms
* above.
*/
.data
.align 4
.globl g_heapbase
.type g_heapbase, object
g_heapbase:
.long _ebss+CONFIG_IDLETHREAD_STACKSIZE
.size g_heapbase, .-g_heapbase
.end
nuttx/arch/arm/src/arm/up_initialstate.c
-146
View File
@@ -1,146 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_initialstate.c
*
* Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <string.h>
#include <nuttx/arch.h>
#include "arm.h"
#include "up_internal.h"
#include "up_arch.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_initial_state
*
* Description:
* A new thread is being started and a new TCB
* has been created. This function is called to initialize
* the processor specific portions of the new TCB.
*
* This function must setup the intial architecture registers
* and/or stack so that execution will begin at tcb->start
* on the next context switch.
*
****************************************************************************/
void up_initial_state(_TCB *tcb)
{
struct xcptcontext *xcp = &tcb->xcp;
uint32_t cpsr;
/* Initialize the initial exception register context structure */
memset(xcp, 0, sizeof(struct xcptcontext));
/* Save the initial stack pointer */
xcp->regs[REG_SP] = (uint32_t)tcb->adj_stack_ptr;
/* Save the task entry point */
xcp->regs[REG_PC] = (uint32_t)tcb->start;
/* If this task is running PIC, then set the PIC base register to the
* address of the allocated D-Space region.
*/
#ifdef CONFIG_PIC
if (tcb->dspace != NULL)
{
/* Set the PIC base register (probably R10) to the address of the
* alloacated D-Space region.
*/
xcp->regs[REG_PIC] = (uint32_t)tcb->dspace->region;
}
#endif
/* Set supervisor- or user-mode, depending on how NuttX is configured and
* what kind of thread is being started. Disable FIQs in any event
*/
#ifdef CONFIG_NUTTX_KERNEL
if ((tcb->flags & TCB_FLAG_TTYPE_MASK) == TCB_FLAG_TTYPE_KERNEL)
{
/* It is a kernel thread.. set supervisor mode */
cpsr = SVC_MODE | PSR_F_BIT;
}
else
{
/* It is a normal task or a pthread. Set user mode */
cpsr = USR_MODE | PSR_F_BIT;
}
#else
/* If the kernel build is not selected, then all threads run in
* supervisor-mode.
*/
cpsr = SVC_MODE | PSR_F_BIT;
#endif
/* Enable or disable interrupts, based on user configuration */
# ifdef CONFIG_SUPPRESS_INTERRUPTS
cpsr |= PSR_I_BIT;
# endif
xcp->regs[REG_CPSR] = cpsr;
}
nuttx/arch/arm/src/arm/up_nommuhead.S
-167
View File
@@ -1,167 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_nommuhead.S
*
* Copyright (C) 2007, 2009-2010, 2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include "arm.h"
#include "up_internal.h"
#include "up_arch.h"
/****************************************************************************
* Macros
****************************************************************************/
/* This macro will modify r0, r1, r2 and r14 */
#ifdef CONFIG_DEBUG
.macro showprogress, code
mov r0, #\code
bl up_lowputc
.endm
#else
.macro showprogress, code
.endm
#endif
/****************************************************************************
* OS Entry Point
****************************************************************************/
/* We assume the bootloader has already initialized most of the h/w for
* us and that only leaves us having to do some os specific things
* below.
*/
.text
.global __start
.type __start, #function
__start:
/* First, setup initial processor mode */
mov r0, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT )
msr cpsr, r0
showprogress 'A'
/* Setup system stack (and get the BSS range) */
adr r0, LC0
ldmia r0, {r4, r5, sp}
/* Clear system BSS section */
mov r0, #0
1: cmp r4, r5
strcc r0, [r4], #4
bcc 1b
showprogress 'B'
/* Copy system .data sections to new home in RAM. */
#ifdef CONFIG_BOOT_RUNFROMFLASH
adr r3, LC2
ldmia r3, {r0, r1, r2}
1: ldmia r0!, {r3 - r10}
stmia r1!, {r3 - r10}
cmp r1, r2
blt 1b
#endif
/* Perform early serial initialization */
mov fp, #0
#ifdef USE_EARLYSERIALINIT
bl up_earlyserialinit
#endif
#ifdef CONFIG_DEBUG
mov r0, #'C'
bl up_putc
mov r0, #'\n'
bl up_putc
#endif
/* Initialize onboard LEDs */
#ifdef CONFIG_ARCH_LEDS
bl up_ledinit
#endif
/* Then jump to OS entry */
b os_start
/* Variables:
* _sbss is the start of the BSS region (see ld.script)
* _ebss is the end of the BSS regsion (see ld.script)
* The idle task stack starts at the end of BSS and is
* of size CONFIG_IDLETHREAD_STACKSIZE. The heap continues
* from there until the end of memory. See g_heapbase
* below.
*/
LC0: .long _sbss
.long _ebss
.long _ebss+CONFIG_IDLETHREAD_STACKSIZE-4
#ifdef CONFIG_BOOT_RUNFROMFLASH
LC2: .long _eronly /* Where .data defaults are stored in FLASH */
.long _sdata /* Where .data needs to reside in SDRAM */
.long _edata
#endif
.size __start, .-__start
/* This global variable is unsigned long g_heapbase and is
* exported from here only because of its coupling to LCO
* above.
*/
.data
.align 4
.globl g_heapbase
.type g_heapbase, object
g_heapbase:
.long _ebss+CONFIG_IDLETHREAD_STACKSIZE
.size g_heapbase, .-g_heapbase
.end
nuttx/arch/arm/src/arm/up_pginitialize.c
-96
View File
@@ -1,96 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_pginitialize.c
* Initialize the MMU for on-demand paging support.
*
* Copyright (C) 2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <debug.h>
#include <nuttx/sched.h>
#include <nuttx/page.h>
#include "up_internal.h"
#ifdef CONFIG_PAGING
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_pginitialize()
*
* Description:
* Initialize the MMU for on-demand paging support..
*
* Input Parameters:
* None.
*
* Returned Value:
* None. This function will crash if any errors are detected during MMU
* initialization
*
* Assumptions:
* - Called early in the platform initialization sequence so that no special
* concurrency protection is required.
*
****************************************************************************/
void up_pginitialize(void)
{
/* None needed at present. This file is just retained in case the need
* arises in the future. Nothing calls up_pginitialize() now. If needed,
* if should be called early in up_boot.c to assure that all paging is
* ready.
*/
}
#endif /* CONFIG_PAGING */
nuttx/arch/arm/src/arm/up_prefetchabort.c
-154
View File
@@ -1,154 +0,0 @@
/****************************************************************************
* arch/arm/src/src/up_prefetchabort.c
*
* Copyright (C) 2007-2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <debug.h>
#include <nuttx/irq.h>
#ifdef CONFIG_PAGING
# include <nuttx/page.h>
#endif
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Debug ********************************************************************/
/* Output debug info if stack dump is selected -- even if
* debug is not selected.
*/
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
# define lldbg lib_lowprintf
#endif
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_prefetchabort
*
* Description;
* This is the prefetch abort exception handler. The ARM prefetch abort
* exception occurs when a memory fault is detected during an an
* instruction fetch.
*
****************************************************************************/
void up_prefetchabort(uint32_t *regs)
{
#ifdef CONFIG_PAGING
uint32_t *savestate;
/* Save the saved processor context in current_regs where it can be accessed
* for register dumps and possibly context switching.
*/
savestate = (uint32_t*)current_regs;
#endif
current_regs = regs;
#ifdef CONFIG_PAGING
/* Get the (virtual) address of instruction that caused the prefetch abort.
* When the exception occurred, this address was provided in the lr register
* and this value was saved in the context save area as the PC at the
* REG_R15 index.
*
* Check to see if this miss address is within the configured range of
* virtual addresses.
*/
pglldbg("VADDR: %08x VBASE: %08x VEND: %08x\n",
regs[REG_PC], PG_PAGED_VBASE, PG_PAGED_VEND);
if (regs[REG_R15] >= PG_PAGED_VBASE && regs[REG_R15] < PG_PAGED_VEND)
{
/* Save the offending PC as the fault address in the TCB of the currently
* executing task. This value is, of course, already known in regs[REG_R15],
* but saving it in this location will allow common paging logic for both
* prefetch and data aborts.
*/
FAR _TCB *tcb = (FAR _TCB *)g_readytorun.head;
tcb->xcp.far = regs[REG_R15];
/* Call pg_miss() to schedule the page fill. A consequences of this
* call are:
*
* (1) The currently executing task will be blocked and saved on
* on the g_waitingforfill task list.
* (2) An interrupt-level context switch will occur so that when
* this function returns, it will return to a different task,
* most likely the page fill worker thread.
* (3) The page fill worker task has been signalled and should
* execute immediately when we return from this exception.
*/
pg_miss();
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
}
else
#endif
{
lldbg("Prefetch abort. PC: %08x\n", regs[REG_PC]);
PANIC(OSERR_ERREXCEPTION);
}
}
nuttx/arch/arm/src/arm/up_releasepending.c
-132
View File
@@ -1,132 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_releasepending.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <sched.h>
#include <debug.h>
#include <nuttx/arch.h>
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_release_pending
*
* Description:
* Release and ready-to-run tasks that have
* collected in the pending task list. This can call a
* context switch if a new task is placed at the head of
* the ready to run list.
*
****************************************************************************/
void up_release_pending(void)
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
slldbg("From TCB=%p\n", rtcb);
/* Merge the g_pendingtasks list into the g_readytorun task list */
/* sched_lock(); */
if (sched_mergepending())
{
/* The currently active task has changed! We will need to
* switch contexts. First check if we are operating in
* interrupt context:
*/
if (current_regs)
{
/* Yes, then we have to do things differently.
* Just copy the current_regs into the OLD rtcb.
*/
up_savestate(rtcb->xcp.regs);
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
slldbg("New Active Task TCB=%p\n", rtcb);
/* Then switch contexts */
up_restorestate(rtcb->xcp.regs);
}
/* Copy the exception context into the TCB of the task that
* was currently active. if up_saveusercontext returns a non-zero
* value, then this is really the previously running task
* restarting!
*/
else if (!up_saveusercontext(rtcb->xcp.regs))
{
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
slldbg("New Active Task TCB=%p\n", rtcb);
/* Then switch contexts */
up_fullcontextrestore(rtcb->xcp.regs);
}
}
}
nuttx/arch/arm/src/arm/up_reprioritizertr.c
-187
View File
@@ -1,187 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_reprioritizertr.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <sched.h>
#include <debug.h>
#include <nuttx/arch.h>
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_reprioritize_rtr
*
* Description:
* Called when the priority of a running or
* ready-to-run task changes and the reprioritization will
* cause a context switch. Two cases:
*
* 1) The priority of the currently running task drops and the next
* task in the ready to run list has priority.
* 2) An idle, ready to run task's priority has been raised above the
* the priority of the current, running task and it now has the
* priority.
*
* Inputs:
* tcb: The TCB of the task that has been reprioritized
* priority: The new task priority
*
****************************************************************************/
void up_reprioritize_rtr(_TCB *tcb, uint8_t priority)
{
/* Verify that the caller is sane */
if (tcb->task_state < FIRST_READY_TO_RUN_STATE ||
tcb->task_state > LAST_READY_TO_RUN_STATE
#if SCHED_PRIORITY_MIN > 0
|| priority < SCHED_PRIORITY_MIN
#endif
#if SCHED_PRIORITY_MAX < UINT8_MAX
|| priority > SCHED_PRIORITY_MAX
#endif
)
{
PANIC(OSERR_BADREPRIORITIZESTATE);
}
else
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
bool switch_needed;
slldbg("TCB=%p PRI=%d\n", tcb, priority);
/* Remove the tcb task from the ready-to-run list.
* sched_removereadytorun will return true if we just
* remove the head of the ready to run list.
*/
switch_needed = sched_removereadytorun(tcb);
/* Setup up the new task priority */
tcb->sched_priority = (uint8_t)priority;
/* Return the task to the specified blocked task list.
* sched_addreadytorun will return true if the task was
* added to the new list. We will need to perform a context
* switch only if the EXCLUSIVE or of the two calls is non-zero
* (i.e., one and only one the calls changes the head of the
* ready-to-run list).
*/
switch_needed ^= sched_addreadytorun(tcb);
/* Now, perform the context switch if one is needed */
if (switch_needed)
{
/* If we are going to do a context switch, then now is the right
* time to add any pending tasks back into the ready-to-run list.
* task list now
*/
if (g_pendingtasks.head)
{
sched_mergepending();
}
/* Are we in an interrupt handler? */
if (current_regs)
{
/* Yes, then we have to do things differently.
* Just copy the current_regs into the OLD rtcb.
*/
up_savestate(rtcb->xcp.regs);
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
slldbg("New Active Task TCB=%p\n", rtcb);
/* Then switch contexts */
up_restorestate(rtcb->xcp.regs);
}
/* Copy the exception context into the TCB at the (old) head of the
* g_readytorun Task list. if up_saveusercontext returns a non-zero
* value, then this is really the previously running task restarting!
*/
else if (!up_saveusercontext(rtcb->xcp.regs))
{
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
slldbg("New Active Task TCB=%p\n", rtcb);
/* Then switch contexts */
up_fullcontextrestore(rtcb->xcp.regs);
}
}
}
}
nuttx/arch/arm/src/arm/up_saveusercontext.S
-119
View File
@@ -1,119 +0,0 @@
/**************************************************************************
* arch/arm/src/arm/up_saveusercontext.S
*
* Copyright (C) 2007, 2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/irq.h>
#include "up_internal.h"
/**************************************************************************
* Private Definitions
**************************************************************************/
/**************************************************************************
* Private Types
**************************************************************************/
/**************************************************************************
* Private Function Prototypes
**************************************************************************/
/**************************************************************************
* Global Variables
**************************************************************************/
/**************************************************************************
* Private Variables
**************************************************************************/
/**************************************************************************
* Private Functions
**************************************************************************/
/**************************************************************************
* Public Functions
**************************************************************************/
/**************************************************************************
* Name: up_saveusercontext
**************************************************************************/
.text
.globl up_saveusercontext
.type up_saveusercontext, function
up_saveusercontext:
/* On entry, a1 (r0) holds address of struct xcptcontext.
* Offset to the user region.
*/
/* Make sure that the return value will be non-zero (the
* value of the other volatile registers don't matter --
* r1-r3, ip). This function is called throught the
* noraml C calling conventions and the values of these
* registers cannot be assumed at the point of setjmp
* return.
*/
mov ip, #1
str ip, [r0, #(4*REG_R0)]
/* Save the volatile registers (plus r12 which really
* doesn't need to be saved)
*/
add r1, r0, #(4*REG_R4)
stmia r1, {r4-r14}
/* Save the current cpsr */
mrs r2, cpsr /* R3 = CPSR value */
add r1, r0, #(4*REG_CPSR)
str r2, [r1]
/* Finally save the return address as the PC so that we
* return to the exit from this function.
*/
add r1, r0, #(4*REG_PC)
str lr, [r1]
/* Return 0 */
mov r0, #0 /* Return value == 0 */
mov pc, lr /* Return */
.size up_saveusercontext, . - up_saveusercontext
nuttx/arch/arm/src/arm/up_schedulesigaction.c
-204
View File
@@ -1,204 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_schedulesigaction.c
*
* Copyright (C) 2007-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <sched.h>
#include <debug.h>
#include <nuttx/arch.h>
#include "arm.h"
#include "os_internal.h"
#include "up_internal.h"
#include "up_arch.h"
#ifndef CONFIG_DISABLE_SIGNALS
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_schedule_sigaction
*
* Description:
* This function is called by the OS when one or more
* signal handling actions have been queued for execution.
* The architecture specific code must configure things so
* that the 'igdeliver' callback is executed on the thread
* specified by 'tcb' as soon as possible.
*
* This function may be called from interrupt handling logic.
*
* This operation should not cause the task to be unblocked
* nor should it cause any immediate execution of sigdeliver.
* Typically, a few cases need to be considered:
*
* (1) This function may be called from an interrupt handler
* During interrupt processing, all xcptcontext structures
* should be valid for all tasks. That structure should
* be modified to invoke sigdeliver() either on return
* from (this) interrupt or on some subsequent context
* switch to the recipient task.
* (2) If not in an interrupt handler and the tcb is NOT
* the currently executing task, then again just modify
* the saved xcptcontext structure for the recipient
* task so it will invoke sigdeliver when that task is
* later resumed.
* (3) If not in an interrupt handler and the tcb IS the
* currently executing task -- just call the signal
* handler now.
*
****************************************************************************/
void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver)
{
/* Refuse to handle nested signal actions */
sdbg("tcb=0x%p sigdeliver=0x%p\n", tcb, sigdeliver);
if (!tcb->xcp.sigdeliver)
{
irqstate_t flags;
/* Make sure that interrupts are disabled */
flags = irqsave();
/* First, handle some special cases when the signal is
* being delivered to the currently executing task.
*/
sdbg("rtcb=0x%p current_regs=0x%p\n", g_readytorun.head, current_regs);
if (tcb == (_TCB*)g_readytorun.head)
{
/* CASE 1: We are not in an interrupt handler and
* a task is signalling itself for some reason.
*/
if (!current_regs)
{
/* In this case just deliver the signal now. */
sigdeliver(tcb);
}
/* CASE 2: We are in an interrupt handler AND the
* interrupted task is the same as the one that
* must receive the signal, then we will have to modify
* the return state as well as the state in the TCB.
*
* Hmmm... there looks like a latent bug here: The following
* logic would fail in the strange case where we are in an
* interrupt handler, the thread is signalling itself, but
* a context switch to another task has occurred so that
* current_regs does not refer to the thread at g_readytorun.head!
*/
else
{
/* Save the return lr and cpsr and one scratch register
* These will be restored by the signal trampoline after
* the signals have been delivered.
*/
tcb->xcp.sigdeliver = sigdeliver;
tcb->xcp.saved_pc = current_regs[REG_PC];
tcb->xcp.saved_cpsr = current_regs[REG_CPSR];
/* Then set up to vector to the trampoline with interrupts
* disabled
*/
current_regs[REG_PC] = (uint32_t)up_sigdeliver;
current_regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT;
/* And make sure that the saved context in the TCB
* is the same as the interrupt return context.
*/
up_savestate(tcb->xcp.regs);
}
}
/* Otherwise, we are (1) signaling a task is not running
* from an interrupt handler or (2) we are not in an
* interrupt handler and the running task is signalling
* some non-running task.
*/
else
{
/* Save the return lr and cpsr and one scratch register
* These will be restored by the signal trampoline after
* the signals have been delivered.
*/
tcb->xcp.sigdeliver = sigdeliver;
tcb->xcp.saved_pc = tcb->xcp.regs[REG_PC];
tcb->xcp.saved_cpsr = tcb->xcp.regs[REG_CPSR];
/* Then set up to vector to the trampoline with interrupts
* disabled
*/
tcb->xcp.regs[REG_PC] = (uint32_t)up_sigdeliver;
tcb->xcp.regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT;
}
irqrestore(flags);
}
}
#endif /* !CONFIG_DISABLE_SIGNALS */
nuttx/arch/arm/src/arm/up_sigdeliver.c
-139
View File
@@ -1,139 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_sigdeliver.c
*
* Copyright (C) 2007-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <sched.h>
#include <debug.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include <arch/board/board.h>
#include "os_internal.h"
#include "up_internal.h"
#include "up_arch.h"
#ifndef CONFIG_DISABLE_SIGNALS
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_sigdeliver
*
* Description:
* This is the a signal handling trampoline. When a signal action was
* posted. The task context was mucked with and forced to branch to this
* location with interrupts disabled.
*
****************************************************************************/
void up_sigdeliver(void)
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
uint32_t regs[XCPTCONTEXT_REGS];
sig_deliver_t sigdeliver;
/* Save the errno. This must be preserved throughout the signal handling
* so that the user code final gets the correct errno value (probably
* EINTR).
*/
int saved_errno = rtcb->pterrno;
up_ledon(LED_SIGNAL);
sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n",
rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);
ASSERT(rtcb->xcp.sigdeliver != NULL);
/* Save the real return state on the stack. */
up_copystate(regs, rtcb->xcp.regs);
regs[REG_PC] = rtcb->xcp.saved_pc;
regs[REG_CPSR] = rtcb->xcp.saved_cpsr;
/* Get a local copy of the sigdeliver function pointer. we do this so that
* we can nullify the sigdeliver function pointer in the TCB and accept
* more signal deliveries while processing the current pending signals.
*/
sigdeliver = rtcb->xcp.sigdeliver;
rtcb->xcp.sigdeliver = NULL;
/* Then restore the task interrupt state */
irqrestore(regs[REG_CPSR]);
/* Deliver the signals */
sigdeliver(rtcb);
/* Output any debug messages BEFORE restoring errno (because they may
* alter errno), then disable interrupts again and restore the original
* errno that is needed by the user logic (it is probably EINTR).
*/
sdbg("Resuming\n");
(void)irqsave();
rtcb->pterrno = saved_errno;
/* Then restore the correct state for this thread of execution. */
up_ledoff(LED_SIGNAL);
up_fullcontextrestore(regs);
}
#endif /* !CONFIG_DISABLE_SIGNALS */
nuttx/arch/arm/src/arm/up_syscall.c
-96
View File
@@ -1,96 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_syscall.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <debug.h>
#include "up_arch.h"
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Output debug info if stack dump is selected -- even if
* debug is not selected.
*/
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
# define lldbg lib_lowprintf
#endif
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* vectors
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_syscall
*
* Description:
* SWI interrupts will vection here with insn=the SWI
* instruction and xcp=the interrupt context
*
* The handler may get the SWI number be de-referencing
* the return address saved in the xcp and decoding
* the SWI instruction
*
****************************************************************************/
void up_syscall(uint32_t *regs)
{
lldbg("Syscall from 0x%x\n", regs[REG_PC]);
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
}
nuttx/arch/arm/src/arm/up_unblocktask.c
-159
View File
@@ -1,159 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_unblocktask.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <sched.h>
#include <debug.h>
#include <nuttx/arch.h>
#include "os_internal.h"
#include "clock_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_unblock_task
*
* Description:
* A task is currently in an inactive task list
* but has been prepped to execute. Move the TCB to the
* ready-to-run list, restore its context, and start execution.
*
* Inputs:
* tcb: Refers to the tcb to be unblocked. This tcb is
* in one of the waiting tasks lists. It must be moved to
* the ready-to-run list and, if it is the highest priority
* ready to run taks, executed.
*
****************************************************************************/
void up_unblock_task(_TCB *tcb)
{
/* Verify that the context switch can be performed */
if ((tcb->task_state < FIRST_BLOCKED_STATE) ||
(tcb->task_state > LAST_BLOCKED_STATE))
{
PANIC(OSERR_BADUNBLOCKSTATE);
}
else
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
/* Remove the task from the blocked task list */
sched_removeblocked(tcb);
/* Reset its timeslice. This is only meaningful for round
* robin tasks but it doesn't here to do it for everything
*/
#if CONFIG_RR_INTERVAL > 0
tcb->timeslice = CONFIG_RR_INTERVAL / MSEC_PER_TICK;
#endif
/* Add the task in the correct location in the prioritized
* g_readytorun task list
*/
if (sched_addreadytorun(tcb))
{
/* The currently active task has changed! We need to do
* a context switch to the new task.
*
* Are we in an interrupt handler?
*/
if (current_regs)
{
/* Yes, then we have to do things differently.
* Just copy the current_regs into the OLD rtcb.
*/
up_savestate(rtcb->xcp.regs);
/* Restore the exception context of the rtcb at the (new) head
* of the g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
/* Then switch contexts */
up_restorestate(rtcb->xcp.regs);
}
/* We are not in an interrupt handler. Copy the user C context
* into the TCB of the task that was previously active. if
* up_saveusercontext returns a non-zero value, then this is really the
* previously running task restarting!
*/
else if (!up_saveusercontext(rtcb->xcp.regs))
{
/* Restore the exception context of the new task that is ready to
* run (probably tcb). This is the new rtcb at the head of the
* g_readytorun task list.
*/
rtcb = (_TCB*)g_readytorun.head;
/* Then switch contexts */
up_fullcontextrestore(rtcb->xcp.regs);
}
}
}
}
nuttx/arch/arm/src/arm/up_undefinedinsn.c
-81
View File
@@ -1,81 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_undefinedinsn.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <debug.h>
#include "os_internal.h"
#include "up_internal.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Output debug info if stack dump is selected -- even if
* debug is not selected.
*/
#ifdef CONFIG_ARCH_STACKDUMP
# undef lldbg
# define lldbg lib_lowprintf
#endif
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_undefinedinsn
****************************************************************************/
void up_undefinedinsn(uint32_t *regs)
{
lldbg("Undefined instruction at 0x%x\n", regs[REG_PC]);
current_regs = regs;
PANIC(OSERR_UNDEFINEDINSN);
}
nuttx/arch/arm/src/arm/up_va2pte.c
-121
View File
@@ -1,121 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_va2pte.c
* Utility to map a virtual address to a L2 page table entry.
*
* Copyright (C) 2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <stdint.h>
#include <debug.h>
#include <nuttx/sched.h>
#include <nuttx/page.h>
#include "chip.h"
#include "pg_macros.h"
#include "up_internal.h"
#ifdef CONFIG_PAGING
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: up_va2pte()
*
* Description:
* Convert a virtual address within the paged text region into a pointer to
* the corresponding page table entry.
*
* Input Parameters:
* vaddr - The virtual address within the paged text region.
*
* Returned Value:
* A pointer to the corresponding page table entry.
*
* Assumptions:
* - This function is called from the normal tasking context (but with
* interrupts disabled). The implementation must take whatever actions
* are necessary to assure that the operation is safe within this
* context.
*
****************************************************************************/
uint32_t *up_va2pte(uintptr_t vaddr)
{
uint32_t L1;
uint32_t *L2;
unsigned int ndx;
/* The virtual address is expected to lie in the paged text region */
DEBUGASSERT(vaddr >= PG_PAGED_VBASE && vaddr < PG_PAGED_VEND);
/* Get the L1 table entry associated with this virtual address */
L1 = *(uint32_t*)PG_POOL_VA2L1VADDR(vaddr);
/* Get the address of the L2 page table from the L1 entry */
L2 = (uint32_t*)PG_POOL_L12VPTABLE(L1);
/* Get the index into the L2 page table. Each L1 entry maps
* 256 x 4Kb or 1024 x 1Kb pages.
*/
ndx = (vaddr & 0x000fffff) >> PAGESHIFT;
/* Return true if this virtual address is mapped. */
return &L2[ndx];
}
#endif /* CONFIG_PAGING */
nuttx/arch/arm/src/arm/up_vectoraddrexcptn.S
-83
View File
@@ -1,83 +0,0 @@
/************************************************************************************
* arch/arm/src/src/up_vectoraddrexceptn.S
*
* Copyright (C) 2008-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <nuttx/irq.h>
#include "up_arch.h"
/************************************************************************************
* Definitions
************************************************************************************/
/************************************************************************************
* Global Data
************************************************************************************/
/************************************************************************************
* Assembly Macros
************************************************************************************/
/************************************************************************************
* Private Functions
************************************************************************************/
.text
/************************************************************************************
* Public Functions
************************************************************************************/
.text
/************************************************************************************
* Name: up_vectoraddrexcption
*
* Description:
* Shouldn't happen. This exception handler is in a separate file from other
* vector handlers because some processors (e.g., lpc2148) do not support the
* the Address Exception vector.
*
************************************************************************************/
.globl up_vectoraddrexcptn
.type up_vectoraddrexcptn, %function
up_vectoraddrexcptn:
b up_vectoraddrexcptn
.size up_vectoraddrexcptn, . - up_vectoraddrexcptn
.end
nuttx/arch/arm/src/arm/up_vectors.S
-446
View File
@@ -1,446 +0,0 @@
/************************************************************************************
* arch/arm/src/arm/up_vectors.S
*
* Copyright (C) 2007-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
#include <nuttx/irq.h>
#include "arm.h"
#include "up_arch.h"
/************************************************************************************
* Definitions
************************************************************************************/
/************************************************************************************
* Global Data
************************************************************************************/
.data
g_irqtmp:
.word 0 /* Saved lr */
.word 0 /* Saved spsr */
g_undeftmp:
.word 0 /* Saved lr */
.word 0 /* Saved spsr */
g_aborttmp:
.word 0 /* Saved lr */
.word 0 /* Saved spsr */
/************************************************************************************
* Assembly Macros
************************************************************************************/
/************************************************************************************
* Private Functions
************************************************************************************/
.text
/************************************************************************************
* Public Functions
************************************************************************************/
.text
/************************************************************************************
* Name: up_vectorirq
*
* Description:
* Interrupt excetpion. Entered in IRQ mode with spsr = SVC CPSR, lr = SVC PC
*
************************************************************************************/
.globl up_vectorirq
.type up_vectorirq, %function
up_vectorirq:
/* On entry, we are in IRQ mode. We are free to use
* the IRQ mode r13 and r14.
*/
ldr r13, .Lirqtmp
sub lr, lr, #4
str lr, [r13] @ save lr_IRQ
mrs lr, spsr
str lr, [r13, #4] @ save spsr_IRQ
/* Then switch back to SVC mode */
bic lr, lr, #MODE_MASK /* Keep F and T bits */
orr lr, lr, #(SVC_MODE | PSR_I_BIT)
msr cpsr_c, lr /* Switch to SVC mode */
/* Create a context structure. First set aside a stack frame
* and store r0-r12 into the frame.
*/
sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SVC mode regs */
/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14
/* Get the values for r15(pc) and CPSR in r3 and r4 */
ldr r0, .Lirqtmp /* Points to temp storage */
ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
/* Then call the IRQ handler with interrupts disabled. */
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
#if CONFIG_ARCH_INTERRUPTSTACK > 3
ldr sp, .Lirqstackbase /* SP = interrupt stack base */
str r0, [sp] /* Save the user stack pointer */
bl up_decodeirq /* Call the handler */
ldr sp, [sp] /* Restore the user stack pointer */
#else
bl up_decodeirq /* Call the handler */
#endif
/* Restore the CPSR, SVC modr registers and return */
.Lnoirqset:
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr, r0
ldmia sp, {r0-r15}^ /* Return */
.Lirqtmp:
.word g_irqtmp
#if CONFIG_ARCH_INTERRUPTSTACK > 3
.Lirqstackbase:
.word up_stackbase
#endif
.size up_vectorirq, . - up_vectorirq
.align 5
/************************************************************************************
* Function: up_vectorswi
*
* Description:
* SWI interrupt. We enter the SWI in SVC mode.
*
************************************************************************************/
.globl up_vectorswi
.type up_vectorswi, %function
up_vectorswi:
/* Create a context structure. First set aside a stack frame
* and store r0-r12 into the frame.
*/
sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SVC mode regs */
/* Get the correct values of r13(sp), r14(lr), r15(pc)
* and CPSR in r1-r4 */
add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14 /* R14 is altered on return from SWI */
mov r3, r14 /* Save r14 as the PC as well */
mrs r4, spsr /* Get the saved CPSR */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
/* Then call the SWI handler with interrupts disabled.
* void up_syscall(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_syscall /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr, r0
ldmia sp, {r0-r15}^ /* Return */
.size up_vectorswi, . - up_vectorswi
.align 5
/************************************************************************************
* Name: up_vectordata
*
* Description:
* This is the data abort exception dispatcher. The ARM data abort exception occurs
* when a memory fault is detected during a data transfer. This handler saves the
* current processor state and gives control to data abort handler. This function
* is entered in ABORT mode with spsr = SVC CPSR, lr = SVC PC
*
************************************************************************************/
.globl up_vectordata
.type up_vectordata, %function
up_vectordata:
/* On entry we are free to use the ABORT mode registers
* r13 and r14
*/
ldr r13, .Ldaborttmp /* Points to temp storage */
sub lr, lr, #8 /* Fixup return */
str lr, [r13] /* Save in temp storage */
mrs lr, spsr /* Get SPSR */
str lr, [r13, #4] /* Save in temp storage */
/* Then switch back to SVC mode */
bic lr, lr, #MODE_MASK /* Keep F and T bits */
orr lr, lr, #(SVC_MODE | PSR_I_BIT)
msr cpsr_c, lr /* Switch to SVC mode */
/* Create a context structure. First set aside a stack frame
* and store r0-r12 into the frame.
*/
sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SVC mode regs */
/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14
/* Get the values for r15(pc) and CPSR in r3 and r4 */
ldr r0, .Ldaborttmp /* Points to temp storage */
ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
/* Then call the data abort handler with interrupts disabled.
* void up_dataabort(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
#ifdef CONFIG_PAGING
mrc p15, 0, r2, c5, c0, 0 /* Get r2=FSR */
mrc p15, 0, r1, c6, c0, 0 /* Get R1=FAR */
#endif
bl up_dataabort /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr_cxsf, r0
ldmia sp, {r0-r15}^ /* Return */
.Ldaborttmp:
.word g_aborttmp
.size up_vectordata, . - up_vectordata
.align 5
/************************************************************************************
* Name: up_vectorprefetch
*
* Description:
* This is the prefetch abort exception dispatcher. The ARM prefetch abort exception
* occurs when a memory fault is detected during an an instruction fetch. This
* handler saves the current processor state and gives control to prefetch abort
* handler. This function is entered in ABT mode with spsr = SVC CPSR, lr = SVC PC.
*
************************************************************************************/
.globl up_vectorprefetch
.type up_vectorprefetch, %function
up_vectorprefetch:
/* On entry we are free to use the ABORT mode registers
* r13 and r14
*/
ldr r13, .Lpaborttmp /* Points to temp storage */
sub lr, lr, #4 /* Fixup return */
str lr, [r13] /* Save in temp storage */
mrs lr, spsr /* Get SPSR */
str lr, [r13, #4] /* Save in temp storage */
/* Then switch back to SVC mode */
bic lr, lr, #MODE_MASK /* Keep F and T bits */
orr lr, lr, #(SVC_MODE | PSR_I_BIT)
msr cpsr_c, lr /* Switch to SVC mode */
/* Create a context structure. First set aside a stack frame
* and store r0-r12 into the frame.
*/
sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SVC mode regs */
/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14
/* Get the values for r15(pc) and CPSR in r3 and r4 */
ldr r0, .Lpaborttmp /* Points to temp storage */
ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
/* Then call the prefetch abort handler with interrupts disabled.
* void up_prefetchabort(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_prefetchabort /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr_cxsf, r0
ldmia sp, {r0-r15}^ /* Return */
.Lpaborttmp:
.word g_aborttmp
.size up_vectorprefetch, . - up_vectorprefetch
.align 5
/************************************************************************************
* Name: up_vectorundefinsn
*
* Description:
* Undefined instruction entry exception. Entered in UND mode, spsr = SVC CPSR,
* lr = SVC PC
*
************************************************************************************/
.globl up_vectorundefinsn
.type up_vectorundefinsn, %function
up_vectorundefinsn:
/* On entry we are free to use the UND mode registers
* r13 and r14
*/
ldr r13, .Lundeftmp /* Points to temp storage */
str lr, [r13] /* Save in temp storage */
mrs lr, spsr /* Get SPSR */
str lr, [r13, #4] /* Save in temp storage */
/* Then switch back to SVC mode */
bic lr, lr, #MODE_MASK /* Keep F and T bits */
orr lr, lr, #(SVC_MODE | PSR_I_BIT)
msr cpsr_c, lr /* Switch to SVC mode */
/* Create a context structure. First set aside a stack frame
* and store r0-r12 into the frame.
*/
sub sp, sp, #XCPTCONTEXT_SIZE
stmia sp, {r0-r12} /* Save the SVC mode regs */
/* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */
add r1, sp, #XCPTCONTEXT_SIZE
mov r2, r14
/* Get the values for r15(pc) and CPSR in r3 and r4 */
ldr r0, .Lundeftmp /* Points to temp storage */
ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */
add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */
stmia r0, {r1-r4}
/* Then call the undef insn handler with interrupts disabled.
* void up_undefinedinsn(struct xcptcontext *xcp)
*/
mov fp, #0 /* Init frame pointer */
mov r0, sp /* Get r0=xcp */
bl up_undefinedinsn /* Call the handler */
/* Restore the CPSR, SVC modr registers and return */
ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */
msr spsr_cxsf, r0
ldmia sp, {r0-r15}^ /* Return */
.Lundeftmp:
.word g_undeftmp
.size up_vectorundefinsn, . - up_vectorundefinsn
.align 5
/************************************************************************************
* Name: up_vectorfiq
*
* Description:
* Shouldn't happen
*
************************************************************************************/
.globl up_vectorfiq
.type up_vectorfiq, %function
up_vectorfiq:
subs pc, lr, #4
.size up_vectorfiq, . - up_vectorfiq
/************************************************************************************
* Name: up_interruptstack/g_userstack
************************************************************************************/
#if CONFIG_ARCH_INTERRUPTSTACK > 3
.bss
.align 4
.globl g_userstack
.type g_userstack, object
up_interruptstack:
.skip ((CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4)
g_userstack:
up_stackbase:
.skip 4
.size g_userstack, 4
.size up_interruptstack, (CONFIG_ARCH_INTERRUPTSTACK & ~3)
#endif
.end
nuttx/arch/arm/src/arm/up_vectortab.S
-103
View File
@@ -1,103 +0,0 @@
/****************************************************************************
* arch/arm/src/arm/up_vectortab.S
*
* Copyright (C) 2007, 2009-2010 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* 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 <nuttx/config.h>
/****************************************************************************
* Definitions
****************************************************************************/
/****************************************************************************
* Global Data
****************************************************************************/
/****************************************************************************
* Assembly Macros
****************************************************************************/
/****************************************************************************
* Name: _vector_start
*
* Description:
* Vector initialization block
****************************************************************************/
.globl _vector_start
/* These will be relocated to VECTOR_BASE. */
_vector_start:
ldr pc, .Lresethandler /* 0x00: Reset */
ldr pc, .Lundefinedhandler /* 0x04: Undefined instruction */
ldr pc, .Lswihandler /* 0x08: Software interrupt */
ldr pc, .Lprefetchaborthandler /* 0x0c: Prefetch abort */
ldr pc, .Ldataaborthandler /* 0x10: Data abort */
ldr pc, .Laddrexcptnhandler /* 0x14: Address exception */
ldr pc, .Lirqhandler /* 0x18: IRQ */
ldr pc, .Lfiqhandler /* 0x1c: FIQ */
.globl __start
.globl up_vectorundefinsn
.globl up_vectorswi
.globl up_vectorprefetch
.globl up_vectordata
.globl up_vectoraddrexcptn
.globl up_vectorirq
.globl up_vectorfiq
.Lresethandler:
.long __start
.Lundefinedhandler:
.long up_vectorundefinsn
.Lswihandler:
.long up_vectorswi
.Lprefetchaborthandler:
.long up_vectorprefetch
.Ldataaborthandler:
.long up_vectordata
.Laddrexcptnhandler:
.long up_vectoraddrexcptn
.Lirqhandler:
.long up_vectorirq
.Lfiqhandler:
.long up_vectorfiq
.globl _vector_end
_vector_end:
.end
nuttx/configs/px4fmu/nsh/appconfig
+4
View File
@@ -52,6 +52,10 @@ CONFIGURED_APPS += systemcmds/boardinfo
CONFIGURED_APPS += systemcmds/mixer
#CONFIGURED_APPS += systemcmds/calibration
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
# CONFIGURED_APPS += examples/px4_simple_app
CONFIGURED_APPS += uORB
ifeq ($(CONFIG_MAVLINK),y)
nuttx/configs/px4fmu/src/up_nsh.c
+1 -14
View File
@@ -236,19 +236,6 @@ int nsh_archinitialize(void)
if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
int mpu_attempts = 0;
int mpu_fail = 0;
while (mpu_attempts < 1)
{
mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU);
mpu_attempts++;
if (mpu_fail == 0) break;
up_udelay(200);
}
if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n");
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);
@@ -320,7 +307,7 @@ int nsh_archinitialize(void)
if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
/* Report back sensor status */
if (acc_fail || gyro_fail || mag_fail || baro_fail || eeprom_fail)
if (gyro_fail || mag_fail || baro_fail || eeprom_fail)
{
up_ledon(LED_AMBER);
}
nuttx/configs/px4io/io/appconfig
+2 -7
View File
@@ -1,8 +1,6 @@
############################################################################
# configs/stm3210e-eval/nsh/appconfig
#
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -14,7 +12,7 @@
# 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
# 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.
#
@@ -33,7 +31,4 @@
#
############################################################################
# Path to px4io app containing the user_start entry point
CONFIGURED_APPS += px4io
CONFIGURED_APPS += systemlib