static int rtswitch_to_rt(rtswitch_context_t *ctx,
unsigned from_idx,
unsigned to_idx)
{
rtswitch_task_t *from, *to;
int rc;
if (from_idx > ctx->tasks_count || to_idx > ctx->tasks_count)
return -EINVAL;
/* to == from is a special case which means
"return to the previous task". */
if (to_idx == from_idx)
to_idx = ctx->error.last_switch.from;
from = &ctx->tasks[from_idx];
to = &ctx->tasks[to_idx];
from->base.flags |= RTSWITCH_RT;
from->last_switch = ++ctx->switches_count;
ctx->error.last_switch.from = from_idx;
ctx->error.last_switch.to = to_idx;
barrier();
if (ctx->pause_us) {
ctx->next_task = to_idx;
barrier();
rtdm_timer_start(&ctx->wake_up_delay,
ctx->pause_us * 1000, 0,
RTDM_TIMERMODE_RELATIVE);
xnpod_lock_sched();
} else
switch (to->base.flags & RTSWITCH_RT) {
case RTSWITCH_NRT:
ctx->utask = to;
barrier();
rtdm_nrtsig_pend(&ctx->wake_utask);
xnpod_lock_sched();
break;
case RTSWITCH_RT:
xnpod_lock_sched();
rtdm_event_signal(&to->rt_synch);
break;
default:
return -EINVAL;
}
rc = rtdm_event_wait(&from->rt_synch);
xnpod_unlock_sched();
if (rc < 0)
return rc;
if (ctx->failed)
return 1;
return 0;
}
void
pwm_up(rtdm_timer_t *timer)
{
int retval;
size_t channel = 0;
for(; channel < RC_NUM; ++channel)
{
// set pwm to high
gpio_set_value(channels[channel].pwm, 1);
// ideal time to put signal down
down_time[channel] = rtdm_clock_read_monotonic() + up_interval[channel];
if(reconfigured[channel])
{
reconfigured[channel] = 0;
rtdm_timer_stop(&down_timer[channel]);
// request timer to fire DELTA ns earlier then needed
retval = rtdm_timer_start(&down_timer[channel],
up_interval[channel] - DELTA,
PERIOD,
RTDM_TIMERMODE_RELATIVE);
if(retval)
rtdm_printk("TB6612FNG: error reconfiguring down-timer #%i: %i\n",
channel, retval);
}
}
}
void inj1_fire(int timer_us)
{
int ret;
ret = rtdm_timer_start(&inj1_timer, (timer_us*1000), 0, RTDM_TIMERMODE_RELATIVE);
if(ret != 0)
{
//FIXME Handle error
}
else
{
gpio_set_value(INJ_OUT_2, 1);
}
}
int
initpwm(void)
{
int i;
int retval;
for(i = 0; i < RC_NUM; i++)
{
up_interval[i] = RANGE_MAP100(ranges[i][0], ranges[i][1], 0);
reconfigured[i] = 0;
}
retval = InitGPIO(channels, sizeof(channels) / sizeof(channels[0]));
if(retval)
{
rtdm_printk("TB6612FNG: GPIO initialization failed\n");
return retval;
}
rtdm_printk("TB6612FNG: GPIO initialized\n");
rtdm_printk("TB6612FNG: Starting PWM generation timers.\n");
retval = rtdm_timer_init(&up_timer, pwm_up, "up timer");
if(retval)
{
rtdm_printk("TB6612FNG: error initializing up-timer: %i\n", retval);
return retval;
}
for(i = 0; i < RC_NUM; i++)
{
retval = rtdm_timer_init(&down_timer[i], pwm_down, "down timer");
if(retval)
{
rtdm_printk("TB6612FNG: error initializing down-timer #%i: %i\n", i, retval);
return retval;
}
}
retval = rtdm_timer_start(&up_timer,
PERIOD, // we will use periodic timer
PERIOD, // PERIOD period
RTDM_TIMERMODE_RELATIVE);
if(retval)
{
rtdm_printk("TB6612FNG: error starting up-timer: %i\n", retval);
return retval;
}
rtdm_printk("TB6612FNG: timers created\n");
return 0;
}
static int rtswitch_to_nrt(rtswitch_context_t *ctx,
unsigned from_idx,
unsigned to_idx)
{
rtswitch_task_t *from, *to;
if (from_idx > ctx->tasks_count || to_idx > ctx->tasks_count)
return -EINVAL;
from = &ctx->tasks[from_idx];
to = &ctx->tasks[to_idx];
from->base.flags &= ~RTSWITCH_RT;
++ctx->switches_count;
ctx->error.last_switch.from = from_idx;
ctx->error.last_switch.to = to_idx;
if (ctx->pause_us) {
ctx->next_task = to_idx;
rtdm_timer_start(&ctx->wake_up_delay,
ctx->pause_us * 1000, 0,
RTDM_TIMERMODE_RELATIVE);
} else
switch (to->base.flags & RTSWITCH_RT) {
case RTSWITCH_NRT:
up(&to->nrt_synch);
break;
case RTSWITCH_RT:
rtdm_event_signal(&to->rt_synch);
break;
default:
return -EINVAL;
}
if (down_interruptible(&from->nrt_synch))
return -EINTR;
if (ctx->failed)
return 1;
return 0;
}
static void rtcfg_client_recv_stage_2_cfg(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_2_cfg *stage_2_cfg;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
size_t data_len;
int ret;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_2_cfg)) {
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_2_cfg frame\n");
kfree_rtskb(rtskb);
return;
}
stage_2_cfg = (struct rtcfg_frm_stage_2_cfg *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_2_cfg));
if (stage_2_cfg->heartbeat_period) {
ret = rtdm_timer_init(&rtcfg_dev->timer, rtcfg_timer, "rtcfg-timer");
if (ret == 0) {
ret = rtdm_timer_start(&rtcfg_dev->timer,
XN_INFINITE,
(nanosecs_rel_t)ntohs(stage_2_cfg->heartbeat_period) *
1000000,
RTDM_TIMERMODE_RELATIVE);
if (ret < 0)
rtdm_timer_destroy(&rtcfg_dev->timer);
}
if (ret < 0)
/*ERRMSG*/rtdm_printk("RTcfg: unable to create timer task\n");
else
set_bit(FLAG_TIMER_STARTED, &rtcfg_dev->flags);
}
/* add server to station list */
if (rtcfg_add_to_station_list(rtcfg_dev,
rtskb->mac.ethernet->h_source, stage_2_cfg->flags) < 0) {
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
RTCFG_DEBUG(1, "RTcfg: unable to process stage_2_cfg frage\n");
kfree_rtskb(rtskb);
return;
}
rtcfg_dev->other_stations = ntohl(stage_2_cfg->stations);
rtcfg_dev->spec.clt.cfg_len = ntohl(stage_2_cfg->cfg_len);
data_len = MIN(rtcfg_dev->spec.clt.cfg_len, rtskb->len);
if (test_bit(RTCFG_FLAG_STAGE_2_DATA, &rtcfg_dev->flags) &&
(data_len > 0)) {
rtcfg_client_queue_frag(ifindex, rtskb, data_len);
rtskb = NULL;
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations)
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_KNOWN);
} else {
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations) {
rtcfg_complete_cmd(ifindex, RTCFG_CMD_ANNOUNCE, 0);
rtcfg_next_main_state(ifindex,
test_bit(RTCFG_FLAG_READY, &rtcfg_dev->flags) ?
RTCFG_MAIN_CLIENT_READY : RTCFG_MAIN_CLIENT_2);
} else
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_FRAMES);
rtcfg_send_ack(ifindex);
}
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
if (rtskb != NULL)
kfree_rtskb(rtskb);
}
static int rtswitch_to_nrt(rtswitch_context_t *ctx,
unsigned from_idx,
unsigned to_idx)
{
rtswitch_task_t *from, *to;
unsigned expected, fp_val;
int fp_check;
if (from_idx > ctx->tasks_count || to_idx > ctx->tasks_count)
return -EINVAL;
/* to == from is a special case which means
"return to the previous task". */
if (to_idx == from_idx)
to_idx = ctx->error.last_switch.from;
from = &ctx->tasks[from_idx];
to = &ctx->tasks[to_idx];
fp_check = ctx->switches_count == from->last_switch + 1
&& ctx->error.last_switch.from == to_idx
&& ctx->error.last_switch.to == from_idx;
from->base.flags &= ~RTSWITCH_RT;
from->last_switch = ++ctx->switches_count;
ctx->error.last_switch.from = from_idx;
ctx->error.last_switch.to = to_idx;
barrier();
if (ctx->pause_us) {
ctx->next_task = to_idx;
barrier();
rtdm_timer_start(&ctx->wake_up_delay,
ctx->pause_us * 1000, 0,
RTDM_TIMERMODE_RELATIVE);
} else
switch (to->base.flags & RTSWITCH_RT) {
case RTSWITCH_NRT:
switch_to_nrt:
up(&to->nrt_synch);
break;
case RTSWITCH_RT:
if (!fp_check || fp_linux_begin() < 0) {
fp_check = 0;
goto signal_nofp;
}
expected = from_idx + 500 +
(ctx->switches_count % 4000000) * 1000;
fp_regs_set(expected);
rtdm_event_signal(&to->rt_synch);
fp_val = fp_regs_check(expected);
fp_linux_end();
if(down_interruptible(&from->nrt_synch))
return -EINTR;
if (ctx->failed)
return 1;
if (fp_val != expected) {
handle_ktask_error(ctx, fp_val);
return 1;
}
from->base.flags &= ~RTSWITCH_RT;
from->last_switch = ++ctx->switches_count;
ctx->error.last_switch.from = from_idx;
ctx->error.last_switch.to = to_idx;
if ((to->base.flags & RTSWITCH_RT) == RTSWITCH_NRT)
goto switch_to_nrt;
expected = from_idx + 500 +
(ctx->switches_count % 4000000) * 1000;
barrier();
fp_linux_begin();
fp_regs_set(expected);
rtdm_event_signal(&to->rt_synch);
fp_val = fp_regs_check(expected);
fp_linux_end();
if (down_interruptible(&from->nrt_synch))
return -EINTR;
if (ctx->failed)
return 1;
if (fp_val != expected) {
handle_ktask_error(ctx, fp_val);
return 1;
}
from->base.flags &= ~RTSWITCH_RT;
from->last_switch = ++ctx->switches_count;
ctx->error.last_switch.from = from_idx;
ctx->error.last_switch.to = to_idx;
barrier();
if ((to->base.flags & RTSWITCH_RT) == RTSWITCH_NRT)
goto switch_to_nrt;
signal_nofp:
rtdm_event_signal(&to->rt_synch);
break;
default:
return -EINVAL;
}
if (down_interruptible(&from->nrt_synch))
return -EINTR;
if (ctx->failed)
return 1;
return 0;
}
