void TASK_mutex_unlock(task_mutex *m) {
ASSERT(m->entries > 0);
//ASSERT(m->owner == task_sys.current);
ASSERT(!m->reentrant && m->entries <= 1);
if (m->entries > 1) {
m->entries--;
TRACE_TASK_MUTEX_EXIT_L(task_sys.current->_ix);
return;
}
TRACE_TASK_MUTEX_EXIT(task_sys.current->_ix);
task_release_lock(m);
task *t = (task *)m->head;
while (t) {
task *next = t->_next;
t->flags &= ~TASK_WAIT;
t->wait_mutex = NULL;
TRACE_TASK_MUTEX_WAKE(t->_ix);
if ((t->flags & TASK_KILLED) == 0) {
TASK_run(t, t->arg, t->arg_p);
}
t = next;
}
m->head = NULL;
m->last = NULL;
}
static void stmpe_adc_irq(u8_t adc, u16_t val) {
STMPE_DBG("stmpe_impl adc irq adc:%08b val:%04x\n", adc, val);
stmpe.val_adc = val;
if (stmpe.adc_cb) {
task *t = TASK_create(stmpe_adc_cb, 0);
ASSERT(t);
TASK_run(t, val, NULL);
}
}
static s32_t sfos_exe(enum sfos_op op) {
s32_t res;
OS_mutex_lock(&sfos.sig_mutex);
sfos.state = op;
TASK_run(sfos.kernel_task, op, sfos.args);
while (sfos.state != FS_OP_IDLE) {
OS_cond_wait(&sfos.cond, &sfos.sig_mutex);
}
res = sfos.sig_res;
OS_mutex_unlock(&sfos.sig_mutex);
return res;
}
/*
* spi device result callback (might be called from irq, flag)
*/
static void spi_flash_callback_spi_result(spi_dev *dev, int res) {
spi_flash_dev *sfd = (spi_flash_dev *)((char*)dev - offsetof(spi_flash_dev, dev));
if (res != SPI_OK) {
DBG(D_SPI, D_WARN, "SPIF cb err i\n", res);
if (sfd->busy_poll) {
TASK_stop_timer(&sfd->timer);
sfd->busy_poll = FALSE;
}
sfd->state = SPI_FLASH_STATE_ERROR;
TASK_run(sfd->task, res, sfd);
return;
}
spi_flash_update_state(sfd);
}
static void app_rover_setup(app_common *com, app_remote *rem, configuration_t *cnf) {
common = com;
remote = rem;
app_cfg = cnf;
#ifdef CONFIG_SPYBOT_HCSR
RANGE_SENS_init(app_rover_radar_cb);
#endif
#ifdef CONFIG_I2C
I2C_config(_I2C_BUS(0), 100000);
STMPE_init();
#ifdef CONFIG_SPYBOT_LSM
task *config_lsm_t = TASK_create(app_rover_setup_lsm, 0);
ASSERT(config_lsm_t);
TASK_run(config_lsm_t, 0, NULL);
#endif
CFG_EE_init(&eeprom_dev, app_rover_cfg_cb);
CFG_EE_load_config();
#endif // CONFIG_I2C
#ifdef CONFIG_SPYBOT_MOTOR
MOTOR_init();
#endif
#ifdef CONFIG_SPYBOT_SERVO
SERVO_init();
#endif
mech_task = TASK_create(app_rover_mech_task, TASK_STATIC);
TASK_start_timer(mech_task, &mech_timer, 0, 0, 0, 20, "mech");
#ifdef CONFIG_SPYBOT_HCSR
radar_task = TASK_create(app_rover_radar_task, TASK_STATIC);
TASK_start_timer(radar_task, &radar_timer, 0, 0, 100, 65, "radar");
#endif // CONFIG_SPYBOT_HCSR
#ifdef CONFIG_SPYBOT_LSM
int i;
for (i = 0; i < 3; i++) {
acc_extremes[i][0] = S16_MAX;
acc_extremes[i][1] = S16_MIN;
mag_extremes[i][0] = S16_MAX;
mag_extremes[i][1] = S16_MIN;
}
#endif
COMRAD_init();
}
static void usb_rx_cb(u16_t avail, void *arg) {
int in_ix = 0;
while ((avail = IO_rx_available(IOUSB)) > 0) {
int len = MIN(avail, sizeof(in) - in_ix);
IO_get_buf(IOUSB, &in[in_ix], len);
in_ix += len;
if (in_ix >= sizeof(in)) break;
}
int i;
for (i = 0; i < in_ix; i++) {
if (in[i] == '\r' || in[i] == '\n') {
task *t = TASK_create(CLI_TASK_on_usb_input, 0);
TASK_run(t, i, NULL);
}
}
}
void STMPE_init(void) {
memset(&stmpe, 0x00, sizeof(stmpe));
stmpe811_handler_open(&stmpe.handler, _I2C_BUS(0), stmpe_gpio_irq, stmpe_adc_irq, stmpe_temp_irq, stmpe_err_cb);
gpio_config(PORTC, PIN13, CLK_50MHZ, IN, AF0, OPENDRAIN, NOPULL);
// cannot do this, will interfere with high prio hsync signal on controller
//gpio_interrupt_config(PORTC, PIN13, stmpe_irq, FLANK_DOWN);
//gpio_interrupt_mask_enable(PORTC, PIN13, TRUE);
// instead, poll in timer
irq_pin_pre_state = gpio_get(PORTC, PIN13) != 0;
stmpe.task_irq = TASK_create(stmpe_task_irq, TASK_STATIC);
ASSERT(stmpe.task_irq);
init = TRUE;
task *t = TASK_create(stmpe_task_config, 0);
ASSERT(t);
TASK_run(t, 0, NULL);
}
void TASK_timer() {
task_timer *cur_timer = task_sys.first_timer;
if (cur_timer == NULL || task_sys.tim_lock) {
return;
}
task_timer *old_timer = NULL;
while (cur_timer && cur_timer->start_time <= SYS_get_time_ms()) {
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
enter_critical();
TQ_ENTER_CRITICAL;
#endif
if (((cur_timer->task->flags & (TASK_RUN | TASK_WAIT)) == 0) && cur_timer->alive) {
// expired, schedule for run
TRACE_TASK_TIMER(cur_timer->_ix);
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_EXIT_CRITICAL;
#endif
TASK_run(cur_timer->task, cur_timer->arg, cur_timer->arg_p);
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_ENTER_CRITICAL;
#endif
}
old_timer = cur_timer;
cur_timer = cur_timer->_next;
task_sys.first_timer = cur_timer;
if (old_timer->recurrent_time && old_timer->alive) {
// recurrent, reinsert
old_timer->start_time += old_timer->recurrent_time; // need to set this before inserting for sorting
task_insert_timer(old_timer, old_timer->start_time);
} else {
old_timer->alive = FALSE;
}
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_EXIT_CRITICAL;
exit_critical();
#endif
}
}
// execute request depending on bit mask
static void stmpe_exe_req(void) {
u32_t req_mask = stmpe.req_mask;
if (req_mask) {
task *t = NULL;
if (req_mask & STMPE_REQ_GPIO) {
STMPE_DBG("stmpe_impl req EXE gpio\n");
t = TASK_create(stmpe_task_gpio, 0);
}
else if (req_mask & STMPE_REQ_ADC) {
STMPE_DBG("stmpe_impl req EXE adc\n");
t = TASK_create(stmpe_task_adc, 0);
}
else if (req_mask & STMPE_REQ_TEMP) {
STMPE_DBG("stmpe_impl req EXE temp\n");
t = TASK_create(stmpe_task_temp, 0);
}
else if (req_mask & STMPE_REQ_INT_STA) {
STMPE_DBG("stmpe_impl req EXE int sta\n");
t = TASK_create(stmpe_task_int_sta, 0);
}
ASSERT(t);
TASK_run(t, 0, NULL);
}
}
/*
* SPI state spinner, called from spi device finished callback
*/
static void spi_flash_update_state(spi_flash_dev *sfd) {
int res = SPI_OK;
if (sfd->busy_poll) {
// polling busy bit
u32_t busy_res = sfd->tmp_buf[0] & (1<<sfd->flash_conf.busy_sr_bit);
bool poll_exceed = sfd->poll_count > sfd->flash_conf.busy_poll_divisor;
if (!poll_exceed && busy_res) {
DBG(D_SPI, D_DEBUG, "SPIF poll: still busy\n");
// still busy, return
return;
} else {
// not busy any longer or timeout, continue state switching
if (poll_exceed) {
DBG(D_SPI, D_WARN, "SPIF poll: poll count exceeded\n");
} else {
DBG(D_SPI, D_DEBUG, "SPIF poll: busy released\n");
}
TASK_stop_timer(&sfd->timer);
sfd->busy_poll = FALSE;
TASK_run(sfd->task, res, sfd);
return;
}
}
switch (sfd->state) {
// opening
case SPI_FLASH_STATE_OPENING_READ_ID: {
u32_t ret_id =
sfd->tmp_buf[2] | (sfd->tmp_buf[1] << 8) | (sfd->tmp_buf[0] << 16);
if (ret_id != sfd->flash_conf.flash_id) {
DBG(D_SPI, D_WARN, "SPIF invalid id %08x != %08x\n", ret_id, sfd->flash_conf.flash_id);
res = SPI_FLASH_ERR_INVALID_ID;
} else {
sfd->state = SPI_FLASH_STATE_OPENED;
sfd->open = TRUE;
TASK_run(sfd->task, res, sfd);
}
}
break;
// close / protect
case SPI_FLASH_STATE_CLOSING:
sfd->state = SPI_FLASH_STATE_CLOSED;
spi_flash_call_task_within_time(sfd, sfd->flash_conf.time_sr_write_ms, res);
break;
// read sr
case SPI_FLASH_STATE_READ_BUSY: {
sfd->state = SPI_FLASH_STATE_OPENED;
u8_t busy_res = sfd->sr_tmp & (1<<sfd->flash_conf.busy_sr_bit);
if (sfd->sr_dst) {
*((u8_t*)sfd->sr_dst) = busy_res;
sfd->sr_dst = 0;
}
TASK_run(sfd->task, res, sfd);
}
break;
// read data
case SPI_FLASH_STATE_READ:
sfd->state = SPI_FLASH_STATE_OPENED;
TASK_run(sfd->task, res, sfd);
break;
// write data
case SPI_FLASH_STATE_WRITE_SEQ:
sfd->state = SPI_FLASH_STATE_WRITE_WAIT;
spi_flash_call_task_within_time(sfd, sfd->flash_conf.time_page_write_ms, res);
break;
// erase sector
case SPI_FLASH_STATE_ERASE_SEQ:
sfd->state = SPI_FLASH_STATE_ERASE_WAIT;
spi_flash_call_task_within_time(sfd, sfd->flash_conf.time_sector_erase_ms, res);
break;
// protect/unprotect
case SPI_FLASH_STATE_WRSR_SEQ:
sfd->state = SPI_FLASH_STATE_WRSR_WAIT;
spi_flash_call_task_within_time(sfd, sfd->flash_conf.time_sr_write_ms, res);
break;
// mass erase
case SPI_FLASH_STATE_MASS_ERASE:
sfd->state = SPI_FLASH_STATE_OPENED;
spi_flash_call_task_within_time(sfd, sfd->flash_conf.time_mass_erase_ms, res);
break;
// other
case SPI_FLASH_STATE_ERROR:
case SPI_FLASH_STATE_OPENED:
case SPI_FLASH_STATE_CLOSED:
// noop
break;
default:
res = SPI_FLASH_ERR_UNDEFINED_STATE;
break;
}
// something went wrong, report error
if (res != SPI_OK) {
if (sfd->busy_poll) {
TASK_stop_timer(&sfd->timer);
sfd->busy_poll = FALSE;
}
sfd->state = SPI_FLASH_STATE_ERROR;
TASK_run(sfd->task, res, sfd);
}
}
static void app_rover_lsm_cb_irq(lsm303_dev *dev, int res) {
TRACE_USR_MSG(0x20);
task *t = TASK_create(app_rover_lsm_cb_task, 0);
ASSERT(t);
TASK_run(t, res, dev);
}
void CLI_uart_check_char(void *a, u8_t c) {
if (c == '\n' || c == '\r') {
task *t = TASK_create(CLI_TASK_on_uart_input, 0);
TASK_run(t, IO_rx_available(IOSTD), NULL);
}
}
static void stmpe_irq(gpio_pin pin) {
if (!stmpe.irq_task_en) {
stmpe.irq_task_en = TRUE;
TASK_run(stmpe.task_irq, 0, NULL);
}
}
void TASK_loop(task* task, u32_t arg, void* arg_p) {
task->flags |= TASK_LOOP;
TASK_run(task, arg, arg_p);
}
