tdma_ioctl.c

硬实时linux补丁rtai下的网络协议栈 最新

/***
 *
 *  rtmac/tdma/tdma_ioctl.c
 *
 *  RTmac - real-time networking media access control subsystem
 *  Copyright (C) 2002      Marc Kleine-Budde <[e-mail]>,
 *                2003-2005 Jan Kiszka <[e-mail]>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 */

#include <linux/module.h>
#include <linux/delay.h>
#include <asm/div64.h>
#include <asm/uaccess.h>

#include <tdma_chrdev.h>
#include <rtmac/rtmac_vnic.h>
#include <rtmac/tdma/tdma.h>


#ifdef CONFIG_RTNET_TDMA_MASTER
static int tdma_ioctl_master(struct rtnet_device *rtdev,
                             struct tdma_config *cfg)
{
    struct tdma_priv    *tdma;
    u64                 cycle_ms;
    unsigned int        table_size;
    int                 ret;


    if (rtdev->mac_priv == NULL) {
        ret = rtmac_disc_attach(rtdev, &tdma_disc);
        if (ret < 0)
            return ret;
    }

    tdma = (struct tdma_priv *)rtdev->mac_priv->disc_priv;
    if (tdma->magic != TDMA_MAGIC) {
        /* note: we don't clean up an unknown discipline */
        return -ENOTTY;
    }

    if (test_bit(TDMA_FLAG_ATTACHED, &tdma->flags)) {
        /* already attached */
        return -EBUSY;
    }

    set_bit(TDMA_FLAG_MASTER, &tdma->flags);

    tdma->cal_rounds = cfg->args.master.cal_rounds;

    /* search at least 3 cycle periods for other masters */
    cycle_ms = cfg->args.master.cycle_period;
    do_div(cycle_ms, 1000000);
    if (cycle_ms == 0)
        cycle_ms = 1;
    msleep(3*cycle_ms);

    if (rtskb_pool_init(&tdma->cal_rtskb_pool,
                        cfg->args.master.max_cal_requests) !=
        cfg->args.master.max_cal_requests) {
        ret = -ENOMEM;
        goto err_out;
    }

    table_size = sizeof(struct tdma_slot *) *
        ((cfg->args.master.max_slot_id >= 1) ?
            cfg->args.master.max_slot_id + 1 : 2);

    tdma->slot_table = (struct tdma_slot **)kmalloc(table_size, GFP_KERNEL);
    if (!tdma->slot_table) {
        ret = -ENOMEM;
        goto err_out;
    }
    tdma->max_slot_id = cfg->args.master.max_slot_id;
    memset(tdma->slot_table, 0, table_size);

    tdma->cycle_period = cfg->args.master.cycle_period;
    tdma->sync_job.ref_count = 0;
    INIT_LIST_HEAD(&tdma->sync_job.entry);

    if (cfg->args.master.backup_sync_offset == 0)
        tdma->sync_job.id = XMIT_SYNC;
    else {
        set_bit(TDMA_FLAG_BACKUP_MASTER, &tdma->flags);
        tdma->sync_job.id     = BACKUP_SYNC;
        tdma->backup_sync_inc =
                cfg->args.master.backup_sync_offset + tdma->cycle_period;
    }

    /* did we detect another active master? */
    if (test_bit(TDMA_FLAG_RECEIVED_SYNC, &tdma->flags)) {
        /* become a slave, we need to calibrate first */
        tdma->sync_job.id = WAIT_ON_SYNC;
    } else {
        if (test_bit(TDMA_FLAG_BACKUP_MASTER, &tdma->flags))
            printk("TDMA: warning, no primary master detected!\n");
        set_bit(TDMA_FLAG_CALIBRATED, &tdma->flags);
        tdma->current_cycle_start = rtdm_clock_read();
    }

    tdma->first_job = tdma->current_job = &tdma->sync_job;

    rtdm_event_signal(&tdma->worker_wakeup);

    set_bit(TDMA_FLAG_ATTACHED, &tdma->flags);

    return 0;

  err_out:
    rtmac_disc_detach(rtdev);
    return ret;
}
#endif /* CONFIG_RTNET_TDMA_MASTER */



static int tdma_ioctl_slave(struct rtnet_device *rtdev,
                            struct tdma_config *cfg)
{
    struct tdma_priv    *tdma;
    unsigned int        table_size;
    int                 ret;


    if (rtdev->mac_priv == NULL) {
        ret = rtmac_disc_attach(rtdev, &tdma_disc);
        if (ret < 0)
            return ret;
    }

    tdma = (struct tdma_priv *)rtdev->mac_priv->disc_priv;
    if (tdma->magic != TDMA_MAGIC) {
        /* note: we don't clean up an unknown discipline */
        return -ENOTTY;
    }

    if (test_bit(TDMA_FLAG_ATTACHED, &tdma->flags)) {
        /* already attached */
        return -EBUSY;
    }

    tdma->cal_rounds = cfg->args.slave.cal_rounds;
    if (tdma->cal_rounds == 0)
        set_bit(TDMA_FLAG_CALIBRATED, &tdma->flags);

    table_size = sizeof(struct tdma_slot *) *
        ((cfg->args.slave.max_slot_id >= 1) ?
            cfg->args.slave.max_slot_id + 1 : 2);

    tdma->slot_table = (struct tdma_slot **)kmalloc(table_size, GFP_KERNEL);
    if (!tdma->slot_table) {
        ret = -ENOMEM;
        goto err_out;
    }
    tdma->max_slot_id = cfg->args.slave.max_slot_id;
    memset(tdma->slot_table, 0, table_size);

    tdma->sync_job.id        = WAIT_ON_SYNC;
    tdma->sync_job.ref_count = 0;
    INIT_LIST_HEAD(&tdma->sync_job.entry);

    tdma->first_job = tdma->current_job = &tdma->sync_job;

    rtdm_event_signal(&tdma->worker_wakeup);

    set_bit(TDMA_FLAG_ATTACHED, &tdma->flags);

    return 0;

  err_out:
    rtmac_disc_detach(rtdev);
    return ret;
}



static int tdma_ioctl_cal_result_size(struct rtnet_device *rtdev,
                                      struct tdma_config *cfg)
{
    struct tdma_priv    *tdma;


    if (rtdev->mac_priv == NULL)
        return -ENOTTY;

    tdma = (struct tdma_priv *)rtdev->mac_priv->disc_priv;
    if (tdma->magic != TDMA_MAGIC)
        return -ENOTTY;

    if (!test_bit(TDMA_FLAG_CALIBRATED, &tdma->flags))
        return tdma->cal_rounds;
    else
        return 0;
}



int start_calibration(struct rt_proc_call *call)
{
    struct tdma_request_cal *req_cal;
    struct tdma_priv        *tdma;
    rtdm_lockctx_t          context;


    req_cal = rtpc_get_priv(call, struct tdma_request_cal);
    tdma    = req_cal->tdma;

    /* there are no slots yet, simply add this job after first_job */
    rtdm_lock_get_irqsave(&tdma->lock, context);
    tdma->calibration_call = call;
    tdma->job_list_revision++;
    list_add(&req_cal->head.entry, &tdma->first_job->entry);
    rtdm_lock_put_irqrestore(&tdma->lock, context);

    return -CALL_PENDING;
}



void copyback_calibration(struct rt_proc_call *call, void *priv_data)
{
    struct tdma_request_cal *req_cal;
    struct tdma_priv        *tdma;
    int                     i;
    u64                     value;
    u64                     average = 0;
    u64                     min = 0x7FFFFFFFFFFFFFFFLL;
    u64                     max = 0;


    req_cal = rtpc_get_priv(call, struct tdma_request_cal);
    tdma    = req_cal->tdma;

    for (i = 0; i < tdma->cal_rounds; i++) {
        value = req_cal->result_buffer[i];
        average += value;
        if (value < min)
            min = value;
        if (value > max)
            max = value;
        if ((req_cal->cal_results) &&
            (copy_to_user(&req_cal->cal_results[i], &value,
                          sizeof(value)) != 0))
            rtpc_set_result(call, -EFAULT);
    }
    do_div(average, tdma->cal_rounds);
    tdma->master_packet_delay_ns = average;

    average += 500;
    do_div(average, 1000);
    min += 500;
    do_div(min, 1000);
    max += 500;
    do_div(max, 1000);
    printk("TDMA: calibrated master-to-slave packet delay: "
           "%ld us (min/max: %ld/%ld us)\n",
           (unsigned long)average, (unsigned long)min,
           (unsigned long)max);
}



void cleanup_calibration(void *priv_data)
{
    struct tdma_request_cal *req_cal;


    req_cal = (struct tdma_request_cal *)priv_data;
    kfree(req_cal->result_buffer);
}



static int tdma_ioctl_set_slot(struct rtnet_device *rtdev,
                               struct tdma_config *cfg)
{
    struct tdma_priv        *tdma;
    int                     id;
    int                     jnt_id;
    struct tdma_slot        *slot, *old_slot;
    struct tdma_job         *job, *prev_job;
    struct tdma_request_cal req_cal;
    struct rtskb            *rtskb;
    unsigned int            job_list_revision;
    rtdm_lockctx_t          context;
    int                     ret;


    if (rtdev->mac_priv == NULL)
        return -ENOTTY;

    tdma = (struct tdma_priv *)rtdev->mac_priv->disc_priv;
    if (tdma->magic != TDMA_MAGIC)
        return -ENOTTY;

    id = cfg->args.set_slot.id;
    if (id > tdma->max_slot_id)
        return -EINVAL;

    if (cfg->args.set_slot.size == 0)
        cfg->args.set_slot.size = rtdev->mtu;
    else if (cfg->args.set_slot.size > rtdev->mtu)
        return -EINVAL;

    jnt_id = cfg->args.set_slot.joint_slot;
    if ((jnt_id >= 0) &&
        ((jnt_id >= tdma->max_slot_id) ||
         (tdma->slot_table[jnt_id] == 0) ||
         (tdma->slot_table[jnt_id]->mtu != cfg->args.set_slot.size)))
        return -EINVAL;

    slot = (struct tdma_slot *)kmalloc(sizeof(struct tdma_slot), GFP_KERNEL);
    if (!slot)
        return -ENOMEM;

    if (!test_bit(TDMA_FLAG_CALIBRATED, &tdma->flags)) {
        req_cal.head.id        = XMIT_REQ_CAL;
        req_cal.head.ref_count = 0;
        req_cal.tdma           = tdma;
        req_cal.offset         = cfg->args.set_slot.offset;
        req_cal.period         = cfg->args.set_slot.period;
        req_cal.phasing        = cfg->args.set_slot.phasing;
        req_cal.cal_rounds     = tdma->cal_rounds;
        req_cal.cal_results    = cfg->args.set_slot.cal_results;

        req_cal.result_buffer =
            kmalloc(req_cal.cal_rounds * sizeof(u64), GFP_KERNEL);