void tdma_worker(void *arg)
{
struct tdma_priv *tdma = (struct tdma_priv *)arg;
struct tdma_job *job;
rtdm_lockctx_t lockctx;
rtdm_event_wait(&tdma->worker_wakeup);
rtdm_lock_get_irqsave(&tdma->lock, lockctx);
job = tdma->first_job;
while (!test_bit(TDMA_FLAG_SHUTDOWN, &tdma->flags)) {
job->ref_count++;
switch (job->id) {
case WAIT_ON_SYNC:
rtdm_lock_put_irqrestore(&tdma->lock, lockctx);
rtdm_event_wait(&tdma->sync_event);
rtdm_lock_get_irqsave(&tdma->lock, lockctx);
break;
case XMIT_REQ_CAL:
job = do_request_cal_job(tdma, REQUEST_CAL_JOB(job), lockctx);
break;
#ifdef CONFIG_RTNET_TDMA_MASTER
case XMIT_SYNC:
do_xmit_sync_job(tdma, lockctx);
break;
case BACKUP_SYNC:
do_backup_sync_job(tdma, lockctx);
break;
case XMIT_RPL_CAL:
job = do_reply_cal_job(tdma, REPLY_CAL_JOB(job), lockctx);
break;
#endif /* CONFIG_RTNET_TDMA_MASTER */
default:
do_slot_job(tdma, SLOT_JOB(job), lockctx);
break;
}
job->ref_count--;
job = tdma->current_job =
list_entry(job->entry.next, struct tdma_job, entry);
}
rtdm_lock_put_irqrestore(&tdma->lock, lockctx);
}
static void rtcfg_rx_task(void *arg)
{
struct rtskb *rtskb;
struct rtcfg_frm_head *frm_head;
struct rtnet_device *rtdev;
while (rtdm_event_wait(&rx_event) == 0)
while ((rtskb = rtskb_dequeue(&rx_queue))) {
rtdev = rtskb->rtdev;
if (rtskb->pkt_type == PACKET_OTHERHOST) {
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
continue;
}
if (rtskb->len < sizeof(struct rtcfg_frm_head)) {
RTCFG_DEBUG(1, "RTcfg: %s() received an invalid frame\n",
__FUNCTION__);
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
continue;
}
frm_head = (struct rtcfg_frm_head *)rtskb->data;
if (rtcfg_do_main_event(rtskb->rtdev->ifindex,
frm_head->id + RTCFG_FRM_STAGE_1_CFG,
rtskb) < 0)
kfree_rtskb(rtskb);
rtdev_dereference(rtdev);
}
}
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;
}
static void rtpc_dispatch_handler(void *arg)
{
struct rt_proc_call *call;
int ret;
while (rtdm_event_wait(&dispatch_event) == 0)
while ((call = rtpc_dequeue_pending_call())) {
ret = call->proc(call);
if (ret != -CALL_PENDING)
rtpc_complete_call(call, ret);
}
}
static ssize_t uart_rd_rt(struct rtdm_dev_context *context,rtdm_user_info_t * user_info, void *buf,size_t nbyte)
{
int err;
int ret=0;
int count;
MY_DEV *up=(MY_DEV *)context->device->device_data;
u8 *tmp;
printk("..............uart_rd_rt start\n");
tmp=(u8 *)rtdm_malloc(nbyte);
up->buf_rx=(u8 *)tmp;
up->buf_len_rx = nbyte;
count =nbyte;
if (!(up->ier & UART_IER_RDI))
{
up->ier |= UART_IER_RDI;
serial_out(up, UART_IER, up->ier);
}
err=rtdm_event_wait(&up->w_event_rx);
if(err<0)
{
dev_err(up->dev,"controller timed out\n");
rtdm_printk("rtdm_event_timedwait: timeout\n");
return -ETIMEDOUT;
}
if(err==0)
{
ret=nbyte;
}
while(count--)
{
*tmp=read_buffer(up);
printk("Receive rd=%x\n",*tmp);
tmp = tmp +1;
}
tmp=tmp-nbyte;
if(rtdm_safe_copy_to_user(user_info,buf,(void *)tmp, nbyte))
rtdm_printk("ERROR : can't copy data from driver\n");
printk("............uart_rd_rt end\n");
rtdm_free(tmp);
return ret;
}
static int rtswitch_pend_rt(rtswitch_context_t *ctx,
unsigned idx)
{
rtswitch_task_t *task;
int rc;
if (idx > ctx->tasks_count)
return -EINVAL;
task = &ctx->tasks[idx];
task->base.flags |= RTSWITCH_RT;
rc = rtdm_event_wait(&task->rt_synch);
if (rc < 0)
return rc;
if (ctx->failed)
return 1;
return 0;
}
static ssize_t uart_wr_rt(struct rtdm_dev_context *context,rtdm_user_info_t * user_info,const void *buf, size_t nbyte)
{
int ret=0;
int err;
int count;
char c;
MY_DEV *up=(MY_DEV *)context->device->device_data;
char *tmp;
up->buf_len_tx = nbyte;
printk("uart_wr_rt start\n");
tmp=rtdm_malloc(nbyte);
if ((rtdm_safe_copy_from_user(user_info,tmp, buf, up->buf_len_tx)))
rtdm_printk("ERROR : can't copy data to driver\n");
count=nbyte;
while(count--)
{
write_buffer(up,*tmp);
tmp=tmp+1;
// up->buf_tx=(char *)tmp;
// printk("up->buf_tx=%x\n",*up->buf_tx);
//enable Trasmitter holding Register
if (!(up->ier & UART_IER_THRI))
{
up->ier |= UART_IER_THRI;
up->systime = rtdm_clock_read();
serial_out(up, UART_IER, up->ier);
}
}
printk("Tx interrupt enable\n");
printk("rtdm_event_wait before\n");
err=rtdm_event_wait(&up->w_event_tx);
if(err<0)
{
dev_err(up->dev,"controller timed out\n");
rtdm_printk("rtdm_event_timedwait: timeout\n");
return -ETIMEDOUT;
}
up->systime1 = rtdm_clock_read();
up->timeout=(up->systime1)-(up->systime);
printk("scheduling latency=%ld\n",up->timeout);
if(err==0)
{
ret=nbyte;
}
printk("rtdm_event_wait after\n");
printk("uart_wr_rt end\n");
rtdm_free(tmp);
return ret;
}
static int rtdmtest_ioctl(struct rtdm_dev_context *context,
rtdm_user_info_t *user_info,
unsigned int request,
void *arg)
{
struct rtdmtest_context *ctx;
struct rttst_rtdmtest_config config_buf, *config;
rtdm_toseq_t toseq_local, *toseq = NULL;
int i, err = 0;
ctx = (struct rtdmtest_context *)context->dev_private;
switch (request) {
case RTTST_RTIOC_RTDMTEST_SEM_TIMEDDOWN:
case RTTST_RTIOC_RTDMTEST_EVENT_TIMEDWAIT:
case RTTST_RTIOC_RTDMTEST_MUTEX_TIMEDTEST:
case RTTST_RTIOC_RTDMTEST_MUTEX_TEST:
config = arg;
if (user_info) {
if (rtdm_safe_copy_from_user
(user_info, &config_buf, arg,
sizeof(struct rttst_rtdmtest_config)) < 0)
return -EFAULT;
config = &config_buf;
}
if (!config->seqcount)
config->seqcount = 1;
if (config->timeout && config->seqcount > 1) {
toseq = &toseq_local;
rtdm_toseq_init(toseq, config->timeout);
}
switch(request) {
case RTTST_RTIOC_RTDMTEST_SEM_TIMEDDOWN:
for (i = 0; i < config->seqcount; i++) {
err = rtdm_sem_timeddown(&ctx->sem,
config->timeout,
toseq);
if (err)
break;
}
break;
case RTTST_RTIOC_RTDMTEST_EVENT_TIMEDWAIT:
for (i = 0; i < config->seqcount; i++) {
err = rtdm_event_timedwait(&ctx->event,
config->timeout,
toseq);
if (err)
break;
}
break;
case RTTST_RTIOC_RTDMTEST_MUTEX_TIMEDTEST:
for (i = 0; i < config->seqcount; i++) {
err = rtdm_mutex_timedlock(&ctx->mutex,
config->timeout,
toseq);
if (err)
break;
if (config->delay_jiffies) {
__set_current_state(TASK_INTERRUPTIBLE);
schedule_timeout(config->delay_jiffies);
}
rtdm_lock_count++;
rtdm_mutex_unlock(&ctx->mutex);
}
break;
case RTTST_RTIOC_RTDMTEST_MUTEX_TEST:
for (i = 0; i < config->seqcount; i++) {
if ((err = rtdm_mutex_lock(&ctx->mutex)))
break;
rtdm_lock_count++;
rtdm_mutex_unlock(&ctx->mutex);
}
break;
}
break;
case RTTST_RTIOC_RTDMTEST_SEM_DOWN:
err = rtdm_sem_down(&ctx->sem);
break;
case RTTST_RTIOC_RTDMTEST_SEM_UP:
rtdm_sem_up(&ctx->sem);
break;
case RTTST_RTIOC_RTDMTEST_SEM_DESTROY:
rtdm_sem_destroy(&ctx->sem);
break;
case RTTST_RTIOC_RTDMTEST_EVENT_WAIT:
err = rtdm_event_wait(&ctx->event);
break;
case RTTST_RTIOC_RTDMTEST_EVENT_SIGNAL:
rtdm_event_signal(&ctx->event);
break;
case RTTST_RTIOC_RTDMTEST_EVENT_DESTROY:
rtdm_event_destroy(&ctx->event);
break;
case RTTST_RTIOC_RTDMTEST_MUTEX_DESTROY:
rtdm_mutex_destroy(&ctx->mutex);
break;
case RTTST_RTIOC_RTDMTEST_MUTEX_GETSTAT:
printk("RTTST_RTIOC_RTDMTEST_MUTEX_GETSTAT\n");
if (user_info)
config = &config_buf;
else
config = arg;
config->seqcount = rtdm_lock_count;
if (user_info) {
if (rtdm_safe_copy_to_user
(user_info, arg, &config_buf,
sizeof(struct rttst_rtdmtest_config)) < 0)
return -EFAULT;
}
break;
case RTTST_RTIOC_RTDMTEST_NRTSIG_PEND:
rtdm_nrtsig_pend(&ctx->nrtsig);
break;
case RTTST_RTIOC_RTDMTEST_TASK_CREATE:
case RTTST_RTIOC_RTDMTEST_TASK_SET_PRIO:
config = arg;
if (user_info) {
if (rtdm_safe_copy_from_user
(user_info, &config_buf, arg,
sizeof(struct rttst_rtdmtest_config)) < 0)
return -EFAULT;
config = &config_buf;
}
if (request == RTTST_RTIOC_RTDMTEST_TASK_CREATE) {
task_period = config->timeout;
rtdm_task_init(&task, "RTDMTEST",
rtdmtest_task, (void *)config,
config->priority, 0);
} else {
rtdm_task_set_priority(&task, config->priority);
}
break;
case RTTST_RTIOC_RTDMTEST_TASK_DESTROY:
rtdm_task_destroy(&task);
rtdm_task_join_nrt(&task, 100);
break;
default:
printk("request=%d\n", request);
err = -ENOTTY;
}
return err;
}
