timerbench.c

xenomai 很好的linux实时补丁

    if (tracer_ioctl(request, user_info, arg))
        return 0;

    ctx = (struct rt_tmbench_context *)context->dev_private;

    switch (request) {
        case RTBNCH_RTIOC_START_TMTEST: {
            struct rtbnch_timerconfig   config_buf;
            struct rtbnch_timerconfig   *config;


            config = (struct rtbnch_timerconfig *)arg;
            if (user_info) {
                if (!rtdm_read_user_ok(user_info, arg,
                                       sizeof(struct rtbnch_timerconfig)) ||
                    rtdm_copy_from_user(user_info, &config_buf, arg,
                                        sizeof(struct rtbnch_timerconfig)))
                    return -EFAULT;

                config = &config_buf;
            }

            down(&ctx->nrt_mutex);

            ctx->period          = config->period;
            ctx->warmup_loops    = config->warmup_loops;
            ctx->samples_per_sec = 1000000000 / ctx->period;
            ctx->histogram_size  = config->histogram_size;
            ctx->freeze_max      = config->freeze_max;

            if (ctx->histogram_size > 0) {
                ctx->histogram_min =
                    kmalloc(3 * ctx->histogram_size * sizeof(long),
                            GFP_KERNEL);
                ctx->histogram_max =
                    ctx->histogram_min + config->histogram_size;
                ctx->histogram_avg =
                    ctx->histogram_max + config->histogram_size;

                if (!ctx->histogram_min) {
                    up(&ctx->nrt_mutex);
                    return -ENOMEM;
                }

                memset(ctx->histogram_min, 0,
                       3 * ctx->histogram_size * sizeof(long));
                ctx->bucketsize = config->histogram_bucketsize;
            }

            ctx->result.overall.min        = 10000000;
            ctx->result.overall.max        = -10000000;
            ctx->result.overall.avg        = 0;
            ctx->result.overall.test_loops = 1;
            ctx->result.overall.overruns   = 0;

            ctx->warmup = 1;

            ctx->curr.min        = 10000000;
            ctx->curr.max        = -10000000;
            ctx->curr.avg        = 0;
            ctx->curr.overruns   = 0;

            rtdm_event_init(&ctx->result_event, 0);

            if (config->mode == RTBNCH_TIMER_TASK) {
                if (!test_bit(RTDM_CLOSING, &context->context_flags)) {
                    ctx->mode = RTBNCH_TIMER_TASK;
                    ret = rtdm_task_init(&ctx->timer_task, "timerbench",
                                         timer_task_proc, ctx,
                                         RTDM_TASK_HIGHEST_PRIORITY, 0);
                }
            } else {
                /* FIXME: convert to RTDM timers */
                xntimer_init(&ctx->timer, timer_proc, ctx);

                ctx->curr.test_loops = 0;

                if (!test_bit(RTDM_CLOSING, &context->context_flags)) {
                    ctx->mode = RTBNCH_TIMER_HANDLER;
                    RTDM_EXECUTE_ATOMICALLY(
                        /* start time: one millisecond from now. */
                        ctx->start_time = rtdm_clock_read() + 1000000;
                        ctx->date       = ctx->start_time + ctx->period;

                        /* FIXME: convert to RTDM timers */
                        xntimer_start(&ctx->timer,
                                xnpod_ns2ticks(ctx->date-rtdm_clock_read()),
                                XN_INFINITE);
                    );
                }
            }

            up(&ctx->nrt_mutex);

            break;
        }

        case RTBNCH_RTIOC_STOP_TMTEST: {
            struct rtbnch_overall_result *usr_res;


            usr_res = (struct rtbnch_overall_result *)arg;

            down(&ctx->nrt_mutex);

            if (ctx->mode < 0) {
                up(&ctx->nrt_mutex);
                return -EINVAL;
            }

            if (ctx->mode == RTBNCH_TIMER_TASK)
                rtdm_task_destroy(&ctx->timer_task);
            else
                /* FIXME: convert to RTDM timers */
                xntimer_destroy(&ctx->timer);

            rtdm_event_destroy(&ctx->result_event);

            ctx->mode = -1;

            ctx->result.overall.avg /=
                    ((ctx->result.overall.test_loops) > 1 ?
                     ctx->result.overall.test_loops : 2) - 1;

            if (user_info) {
                if (!rtdm_rw_user_ok(user_info, usr_res,
                                     sizeof(struct rtbnch_overall_result)) ||
                    rtdm_copy_to_user(user_info, &usr_res->result,
                                      &ctx->result.overall,
                                      sizeof(struct rtbnch_result)))
                    ret = -EFAULT;
            } else {
                memcpy(&usr_res->result, &ctx->result.overall,
                       sizeof(struct rtbnch_result));
            }

            if (ctx->histogram_size) {
                int size = ctx->histogram_size * sizeof(long);

                if (user_info) {
                    if (!rtdm_rw_user_ok(user_info, usr_res->histogram_min,
                                         size) ||
                        rtdm_copy_to_user(user_info, usr_res->histogram_min,
                                          ctx->histogram_min, size) ||
                        !rtdm_rw_user_ok(user_info, usr_res->histogram_max,
                                         size) ||
                        rtdm_copy_to_user(user_info, usr_res->histogram_max,
                                          ctx->histogram_max, size) ||
                        !rtdm_rw_user_ok(user_info, usr_res->histogram_avg,
                                         size) ||
                        rtdm_copy_to_user(user_info, usr_res->histogram_avg,
                                          ctx->histogram_avg, size))
                        ret = -EFAULT;
                } else {
                    memcpy(usr_res->histogram_min, ctx->histogram_min, size);
                    memcpy(usr_res->histogram_max, ctx->histogram_max, size);
                    memcpy(usr_res->histogram_avg, ctx->histogram_avg, size);
                }

                kfree(ctx->histogram_min);
            }

            up(&ctx->nrt_mutex);

            break;
        }

        case RTBNCH_RTIOC_INTERM_RESULT:
            ret = -ENOSYS;
            break;

        default:
            ret = -ENOTTY;
    }

    return ret;
}


int rt_tmbench_ioctl_rt(struct rtdm_dev_context *context,
                        rtdm_user_info_t *user_info, int request, void *arg)
{
    struct rt_tmbench_context   *ctx;
    int                         ret = 0;


    if (tracer_ioctl(request, user_info, arg))
        return 0;

    ctx = (struct rt_tmbench_context *)context->dev_private;

    switch (request) {
        case RTBNCH_RTIOC_INTERM_RESULT: {
            struct rtbnch_interm_result *usr_res;


            usr_res = (struct rtbnch_interm_result *)arg;

            ret = rtdm_event_wait(&ctx->result_event);
            if (ret < 0)
                return ret;

            if (user_info) {
                if (!rtdm_rw_user_ok(user_info, usr_res,
                                     sizeof(struct rtbnch_interm_result)) ||
                    rtdm_copy_to_user(user_info, usr_res, &ctx->result,
                                      sizeof(struct rtbnch_interm_result)))
                    ret = -EFAULT;
            } else
                memcpy(usr_res, &ctx->result,
                       sizeof(struct rtbnch_interm_result));

            break;
        }

        case RTBNCH_RTIOC_START_TMTEST:
        case RTBNCH_RTIOC_STOP_TMTEST:
            ret = -ENOSYS;
            break;

        default:
            ret = -ENOTTY;
    }

    return ret;
}


static struct rtdm_device device = {
    struct_version:     RTDM_DEVICE_STRUCT_VER,

    device_flags:       RTDM_NAMED_DEVICE | RTDM_EXCLUSIVE,
    context_size:       sizeof(struct rt_tmbench_context),
    device_name:        "",

    open_rt:            NULL,
    open_nrt:           rt_tmbench_open,

    ops: {
        close_rt:       NULL,
        close_nrt:      rt_tmbench_close,

        ioctl_rt:       rt_tmbench_ioctl_rt,
        ioctl_nrt:      rt_tmbench_ioctl_nrt,

        read_rt:        NULL,
        read_nrt:       NULL,

        write_rt:       NULL,
        write_nrt:      NULL,

        recvmsg_rt:     NULL,
        recvmsg_nrt:    NULL,

        sendmsg_rt:     NULL,
        sendmsg_nrt:    NULL,
    },

    device_class:       RTDM_CLASS_BENCHMARK,
    device_sub_class:   RTDM_SUBCLASS_TIMER,
    driver_name:        "xeno_timerbench",
    driver_version:     RTDM_DRIVER_VER(0, 1, 0),
    peripheral_name:    "Timer Latency Benchmark",
    provider_name:      "Jan Kiszka",
    proc_name:          device.device_name,
};

int __init __timerbench_init(void)
{
    snprintf(device.device_name, RTDM_MAX_DEVNAME_LEN, "rtbenchmark%d",
             start_index);

    return rtdm_dev_register(&device);
}


void __timerbench_exit(void)
{
    rtdm_dev_unregister(&device, 1000);
}


module_init(__timerbench_init);
module_exit(__timerbench_exit);