static void
wakeup_all_tdaemons(void)
{
int i;
for (i = 0; i < _sd_num_daemons; i++)
daemon_wakeup(i);
}
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
switch (page) {
/* handle bulk controls input */
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_controls[offset] = *values;
offset++;
num_values--;
values++;
}
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
// wake up daemon to trigger mixer
daemon_wakeup();
break;
/* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servos[offset] = *values;
offset++;
num_values--;
values++;
}
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
// wake up the main thread to trigger mixer
daemon_wakeup();
break;
/* handle setup for servo failsafe values */
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
offset++;
num_values--;
values++;
}
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
break;
default:
/* avoid offset wrap */
if ((offset + num_values) > 255)
num_values = 255 - offset;
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
break;
offset++;
values++;
}
}
}
static int
_fork_test_daemon(int num_disks, int test_typ, int loop_cnt, int from, int seed)
{
int i;
int type;
int dowait = 0, verify = 0;
if (num_disks == -1) {
return (test_control(test_typ, loop_cnt, from, seed));
}
type = test_typ;
cmn_err(CE_NOTE,
"!sd_test %d %d %d %d %d", num_disks, type, loop_cnt, from, seed);
if (type == 100) {
test_stop = 1;
return (0);
}
if (type == 99) {
/* Set some parameters for other tests */
switch (num_disks) {
/* Params set for this test */
#if 0
case 302 :
_sd_write_len = loop_cnt;
break;
case 303 :
_sd_write_len = loop_cnt;
break;
case 304 :
_sd_trk_zero = loop_cnt;
_sd_trk_size = from;
break;
case 305 :
_sd_min_blks = loop_cnt;
_sd_max_blks = from;
break;
#endif
default :
cmn_err(CE_WARN,
"!Usage : sd_test <test_num> 99"
" <param1> <param2> <param3>");
break;
}
return (0);
} /* type == 99 */
if (type > 1000) {
dowait = 1;
type -= 1000;
}
if (type > 1000) {
verify = 1;
type -= 1000;
}
again:
set_parameters();
for (i = from; i < (from+num_disks); i++) {
if (daemon_awake(i)) {
cmn_err(CE_WARN, "!Daemon %d awake!?", i);
return (-1);
}
if (daemon_nexist(i)) {
cmn_err(CE_WARN, "!Daemon %d nexist!?", i);
return (-1);
}
gld[i].type = type;
gld[i].loop = loop_cnt;
gld[i].seed = seed;
daemon_wakeup(i);
}
cmn_err(CE_CONT, "!%d daemons woken (test %d)\n", num_disks, type);
if (num_disks <= 0)
return (0);
if (dowait) {
wait:
mutex_enter(&tdaemon_lock);
if (!cv_wait_sig(&_wait_daemons, &tdaemon_lock)) {
mutex_exit(&tdaemon_lock);
test_stop = 1;
cmn_err(CE_WARN, "!Interrupt: stopping tests");
return (-1); /* interrupt */
}
mutex_exit(&tdaemon_lock);
/* wait for all to stop */
if (test_stop)
return (-1);
for (i = from; i < (from+num_disks); i++) {
if (daemon_awake(i))
goto wait;
}
}
if (verify) {
verify = 0;
type++; /* next test */
goto again;
}
return (0);
}
