diff options
63 files changed, 789 insertions, 6649 deletions
@@ -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 diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index d626ca213..d3cf8b506 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -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 diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile index 2038e982b..a6d471567 100644 --- a/apps/ardrone_control/Makefile +++ b/apps/ardrone_control/Makefile @@ -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 diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index d457fc2ec..bc5598f9c 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -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 */ diff --git a/apps/commander/Makefile b/apps/commander/Makefile index a1149eb8d..f2972ac4e 100644 --- a/apps/commander/Makefile +++ b/apps/commander/Makefile @@ -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 diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 19fc81f63..149a662fd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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, ¤t_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, ¤t_status); + mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); do_state_update(status_pub, ¤t_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, ¤t_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, ¤t_status); + mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); do_state_update(status_pub, ¤t_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(¤t_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), ¤t_status); + /* publish current state machine */ state_machine_publish(stat_pub, ¤t_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, ¤t_status, mavlink_fd); + //update_state_machine_got_position_fix(stat_pub, ¤t_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, ¤t_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, ¤t_status, SYSTEM_STATE_STANDBY, mavlink_fd); + do_state_update(stat_pub, ¤t_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, ¤t_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); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a62b1437a..3209ee728 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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; } diff --git a/apps/px4/sensors_bringup/Makefile b/apps/examples/px4_simple_app/Makefile index 8867653a0..102f51fd4 100644 --- a/apps/px4/sensors_bringup/Makefile +++ b/apps/examples/px4_simple_app/Makefile @@ -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 diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c new file mode 100644 index 000000000..fd34a5dac --- /dev/null +++ b/apps/examples/px4_simple_app/px4_simple_app.c @@ -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; +} diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 66b3865cc..b6fdc1a3e 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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); + usage(""); return 0; - } - - /* UART device ID */ - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 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); +} diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile index cf971ae05..2c1cfc510 100644 --- a/apps/px4/attitude_estimator_bm/Makefile +++ b/apps/px4/attitude_estimator_bm/Makefile @@ -37,6 +37,6 @@ APPNAME = attitude_estimator_bm PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 4096 +STACKSIZE = 12000 include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index af7ad7187..c917ff7b2 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -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); diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index fafec77f9..a5def874d 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -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; diff --git a/apps/px4/sensors_bringup/.context b/apps/px4/sensors_bringup/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/px4/sensors_bringup/.context +++ /dev/null diff --git a/apps/px4/sensors_bringup/bma180.c b/apps/px4/sensors_bringup/bma180.c deleted file mode 100644 index 6c4b9d483..000000000 --- a/apps/px4/sensors_bringup/bma180.c +++ /dev/null @@ -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; -} diff --git a/apps/px4/sensors_bringup/l3gd20.c b/apps/px4/sensors_bringup/l3gd20.c deleted file mode 100644 index 5bdc10e06..000000000 --- a/apps/px4/sensors_bringup/l3gd20.c +++ /dev/null @@ -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; -} diff --git a/apps/px4/sensors_bringup/sensors.h b/apps/px4/sensors_bringup/sensors.h deleted file mode 100644 index 5fc2fbb08..000000000 --- a/apps/px4/sensors_bringup/sensors.h +++ /dev/null @@ -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 */ diff --git a/apps/px4/sensors_bringup/sensors_main.c b/apps/px4/sensors_bringup/sensors_main.c deleted file mode 100644 index 3aa21ae14..000000000 --- a/apps/px4/sensors_bringup/sensors_main.c +++ /dev/null @@ -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; -} diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index af1213711..91397d11c 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -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); diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index 06be90a0c..9b63d3ac8 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -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 diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 92558a61d..96e0ca350 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -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)
\ No newline at end of file diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index dadc2993d..125839bb3 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,6 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index d847c4ffc..564ee4f8c 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -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); @@ -134,6 +138,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. * * @return 0 on success, != 0 on failure @@ -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); +} + diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c index cbbeb8cc0..e6248dd43 100644 --- a/apps/systemcmds/top/top.c +++ b/apps/systemcmds/top/top.c @@ -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); diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index 13cf8271e..172eef979 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -35,7 +35,8 @@ # System utility library # -CSRCS = hx_stream.c \ +CSRCS = err.c \ + hx_stream.c \ perf_counter.c # diff --git a/apps/systemlib/err.c b/apps/systemlib/err.c new file mode 100644 index 000000000..9692f03dd --- /dev/null +++ b/apps/systemlib/err.c @@ -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); +} diff --git a/apps/px4/px4io/protocol.h b/apps/systemlib/err.h index c186c5b86..bc390d233 100644 --- a/apps/px4/px4io/protocol.h +++ b/apps/systemlib/err.h @@ -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. - */ - -#define PX4IO_OUTPUT_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 2 +#ifndef _SYSTEMLIB_ERR_H +#define _SYSTEMLIB_ERR_H -#pragma pack(push,1) +#include <stdarg.h> -/* command from FMU to IO */ -struct px4io_command { - uint16_t f2i_magic; -#define F2I_MAGIC 0x636d +__BEGIN_DECLS - uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; - bool relay_state[PX4IO_RELAY_CHANNELS]; - bool arm_ok; -}; +__EXPORT const char *getprogname(void); -/* report from IO to FMU */ -struct px4io_report { - uint16_t i2f_magic; -#define I2F_MAGIC 0x7570 +__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))); - uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; - bool armed; - uint8_t channel_count; -} __attribute__((packed)); +__END_DECLS -#pragma pack(pop) +#endif diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 990c067fd..60b71c60a 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -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++) diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c index 609cf5d34..b1a8de7bd 100644 --- a/apps/systemlib/systemlib.c +++ b/apps/systemlib/systemlib.c @@ -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; diff --git a/apps/uORB/parameter_storage.h b/apps/uORB/parameter_storage.h index 54ae63cb9..aa4c1bb4f 100644 --- a/apps/uORB/parameter_storage.h +++ b/apps/uORB/parameter_storage.h @@ -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! diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 8b2b6525c..c378b05f1 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -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; + + + /* system flags - these represent the state predicates */ - uint8_t mode; + 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 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 */ + // 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; diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 270e3861c..1e7cdc8db 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -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; } diff --git a/nuttx/arch/arm/src/arm/arm.h b/nuttx/arch/arm/src/arm/arm.h deleted file mode 100644 index 433283814..000000000 --- a/nuttx/arch/arm/src/arm/arm.h +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/pg_macros.h b/nuttx/arch/arm/src/arm/pg_macros.h deleted file mode 100644 index fc50f6146..000000000 --- a/nuttx/arch/arm/src/arm/pg_macros.h +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_allocpage.c b/nuttx/arch/arm/src/arm/up_allocpage.c deleted file mode 100755 index c2a31d09c..000000000 --- a/nuttx/arch/arm/src/arm/up_allocpage.c +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_assert.c b/nuttx/arch/arm/src/arm/up_assert.c deleted file mode 100644 index f52dc1b94..000000000 --- a/nuttx/arch/arm/src/arm/up_assert.c +++ /dev/null @@ -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*)¤t_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); -} diff --git a/nuttx/arch/arm/src/arm/up_blocktask.c b/nuttx/arch/arm/src/arm/up_blocktask.c deleted file mode 100755 index 36c8740d6..000000000 --- a/nuttx/arch/arm/src/arm/up_blocktask.c +++ /dev/null @@ -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); - } - } - } -} diff --git a/nuttx/arch/arm/src/arm/up_cache.S b/nuttx/arch/arm/src/arm/up_cache.S deleted file mode 100644 index 05926fbb4..000000000 --- a/nuttx/arch/arm/src/arm/up_cache.S +++ /dev/null @@ -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 - diff --git a/nuttx/arch/arm/src/arm/up_checkmapping.c b/nuttx/arch/arm/src/arm/up_checkmapping.c deleted file mode 100755 index 370c94c9d..000000000 --- a/nuttx/arch/arm/src/arm/up_checkmapping.c +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_copystate.c b/nuttx/arch/arm/src/arm/up_copystate.c deleted file mode 100644 index 44f027b32..000000000 --- a/nuttx/arch/arm/src/arm/up_copystate.c +++ /dev/null @@ -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++; - } -} - diff --git a/nuttx/arch/arm/src/arm/up_dataabort.c b/nuttx/arch/arm/src/arm/up_dataabort.c deleted file mode 100644 index c36970c1b..000000000 --- a/nuttx/arch/arm/src/arm/up_dataabort.c +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_doirq.c b/nuttx/arch/arm/src/arm/up_doirq.c deleted file mode 100644 index 1f1c77473..000000000 --- a/nuttx/arch/arm/src/arm/up_doirq.c +++ /dev/null @@ -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); -} diff --git a/nuttx/arch/arm/src/arm/up_fullcontextrestore.S b/nuttx/arch/arm/src/arm/up_fullcontextrestore.S deleted file mode 100644 index d0745ef5b..000000000 --- a/nuttx/arch/arm/src/arm/up_fullcontextrestore.S +++ /dev/null @@ -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 - diff --git a/nuttx/arch/arm/src/arm/up_head.S b/nuttx/arch/arm/src/arm/up_head.S deleted file mode 100644 index c04dddf8a..000000000 --- a/nuttx/arch/arm/src/arm/up_head.S +++ /dev/null @@ -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 - diff --git a/nuttx/arch/arm/src/arm/up_initialstate.c b/nuttx/arch/arm/src/arm/up_initialstate.c deleted file mode 100644 index 4711c9f44..000000000 --- a/nuttx/arch/arm/src/arm/up_initialstate.c +++ /dev/null @@ -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; -} - diff --git a/nuttx/arch/arm/src/arm/up_nommuhead.S b/nuttx/arch/arm/src/arm/up_nommuhead.S deleted file mode 100644 index aac95b73a..000000000 --- a/nuttx/arch/arm/src/arm/up_nommuhead.S +++ /dev/null @@ -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 - diff --git a/nuttx/arch/arm/src/arm/up_pginitialize.c b/nuttx/arch/arm/src/arm/up_pginitialize.c deleted file mode 100755 index 1aea95113..000000000 --- a/nuttx/arch/arm/src/arm/up_pginitialize.c +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_prefetchabort.c b/nuttx/arch/arm/src/arm/up_prefetchabort.c deleted file mode 100644 index ed3dd91d3..000000000 --- a/nuttx/arch/arm/src/arm/up_prefetchabort.c +++ /dev/null @@ -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); - } -} diff --git a/nuttx/arch/arm/src/arm/up_releasepending.c b/nuttx/arch/arm/src/arm/up_releasepending.c deleted file mode 100755 index dcad40159..000000000 --- a/nuttx/arch/arm/src/arm/up_releasepending.c +++ /dev/null @@ -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); - } - } -} diff --git a/nuttx/arch/arm/src/arm/up_reprioritizertr.c b/nuttx/arch/arm/src/arm/up_reprioritizertr.c deleted file mode 100755 index 38bce2a72..000000000 --- a/nuttx/arch/arm/src/arm/up_reprioritizertr.c +++ /dev/null @@ -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); - } - } - } -} diff --git a/nuttx/arch/arm/src/arm/up_saveusercontext.S b/nuttx/arch/arm/src/arm/up_saveusercontext.S deleted file mode 100644 index 8d154d187..000000000 --- a/nuttx/arch/arm/src/arm/up_saveusercontext.S +++ /dev/null @@ -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 - diff --git a/nuttx/arch/arm/src/arm/up_schedulesigaction.c b/nuttx/arch/arm/src/arm/up_schedulesigaction.c deleted file mode 100644 index 0dfb6e540..000000000 --- a/nuttx/arch/arm/src/arm/up_schedulesigaction.c +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_sigdeliver.c b/nuttx/arch/arm/src/arm/up_sigdeliver.c deleted file mode 100644 index f92f85e7e..000000000 --- a/nuttx/arch/arm/src/arm/up_sigdeliver.c +++ /dev/null @@ -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 */ - diff --git a/nuttx/arch/arm/src/arm/up_syscall.c b/nuttx/arch/arm/src/arm/up_syscall.c deleted file mode 100644 index f331a1314..000000000 --- a/nuttx/arch/arm/src/arm/up_syscall.c +++ /dev/null @@ -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); -} diff --git a/nuttx/arch/arm/src/arm/up_unblocktask.c b/nuttx/arch/arm/src/arm/up_unblocktask.c deleted file mode 100755 index 73e292561..000000000 --- a/nuttx/arch/arm/src/arm/up_unblocktask.c +++ /dev/null @@ -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); - } - } - } -} diff --git a/nuttx/arch/arm/src/arm/up_undefinedinsn.c b/nuttx/arch/arm/src/arm/up_undefinedinsn.c deleted file mode 100644 index 4c50991b0..000000000 --- a/nuttx/arch/arm/src/arm/up_undefinedinsn.c +++ /dev/null @@ -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); -} diff --git a/nuttx/arch/arm/src/arm/up_va2pte.c b/nuttx/arch/arm/src/arm/up_va2pte.c deleted file mode 100755 index 5f92ad821..000000000 --- a/nuttx/arch/arm/src/arm/up_va2pte.c +++ /dev/null @@ -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 */ diff --git a/nuttx/arch/arm/src/arm/up_vectoraddrexcptn.S b/nuttx/arch/arm/src/arm/up_vectoraddrexcptn.S deleted file mode 100644 index e034c394f..000000000 --- a/nuttx/arch/arm/src/arm/up_vectoraddrexcptn.S +++ /dev/null @@ -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 diff --git a/nuttx/arch/arm/src/arm/up_vectors.S b/nuttx/arch/arm/src/arm/up_vectors.S deleted file mode 100644 index 00c5d52b0..000000000 --- a/nuttx/arch/arm/src/arm/up_vectors.S +++ /dev/null @@ -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 diff --git a/nuttx/arch/arm/src/arm/up_vectortab.S b/nuttx/arch/arm/src/arm/up_vectortab.S deleted file mode 100644 index a7972fa3c..000000000 --- a/nuttx/arch/arm/src/arm/up_vectortab.S +++ /dev/null @@ -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 diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 36db854e3..4f0c5093f 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -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) diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index 6e092e3b4..d089f2df3 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -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); } diff --git a/nuttx/configs/px4io/io/appconfig b/nuttx/configs/px4io/io/appconfig index 94176c6dc..fbe8307a7 100644 --- a/nuttx/configs/px4io/io/appconfig +++ b/nuttx/configs/px4io/io/appconfig @@ -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 |