tdma_proto.c

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

/***
 *
 *  rtmac/tdma/tdma_proto.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/init.h>
#include "asm/div64.h"

#include <rtdev.h>
#include <rtmac/rtmac_proto.h>
#include <rtmac/tdma/tdma_proto.h>


void tdma_xmit_sync_frame(struct tdma_priv *tdma)
{
    struct rtnet_device     *rtdev = tdma->rtdev;
    struct rtskb            *rtskb;
    struct tdma_frm_sync    *sync;


    rtskb = alloc_rtskb(rtdev->hard_header_len + sizeof(struct rtmac_hdr) +
                        sizeof(struct tdma_frm_sync) + 15, &global_pool);
    if (!rtskb)
        goto err_out;

    rtskb_reserve(rtskb,
        (rtdev->hard_header_len + sizeof(struct rtmac_hdr) + 15) & ~15);

    sync = (struct tdma_frm_sync *)rtskb_put(rtskb,
                                             sizeof(struct tdma_frm_sync));

    if (rtmac_add_header(rtdev, rtdev->broadcast,
                         rtskb, RTMAC_TYPE_TDMA, 0) < 0) {
        kfree_rtskb(rtskb);
        goto err_out;
    }

    sync->head.version = __constant_htons(TDMA_FRM_VERSION);
    sync->head.id      = __constant_htons(TDMA_FRM_SYNC);

    sync->cycle_no         = htonl(tdma->current_cycle);
    sync->xmit_stamp       = tdma->clock_offset;
    sync->sched_xmit_stamp =
            cpu_to_be64(tdma->clock_offset + tdma->current_cycle_start);

    rtskb->xmit_stamp = &sync->xmit_stamp;

    rtmac_xmit(rtskb);

    /* signal local waiters */
    rtdm_event_pulse(&tdma->sync_event);

    return;

  err_out:
    /*ERROR*/rtdm_printk("TDMA: Failed to transmit sync frame!\n");
    return;
}



int tdma_xmit_request_cal_frame(struct tdma_priv *tdma, u32 reply_cycle,
                                u64 reply_slot_offset)
{
    struct rtnet_device     *rtdev = tdma->rtdev;
    struct rtskb            *rtskb;
    struct tdma_frm_req_cal *req_cal;
    int                     ret;


    rtskb = alloc_rtskb(rtdev->hard_header_len + sizeof(struct rtmac_hdr) +
                        sizeof(struct tdma_frm_req_cal) + 15, &global_pool);
    ret = -ENOMEM;
    if (!rtskb)
        goto err_out;

    rtskb_reserve(rtskb,
        (rtdev->hard_header_len + sizeof(struct rtmac_hdr) + 15) & ~15);

    req_cal = (struct tdma_frm_req_cal *)
        rtskb_put(rtskb, sizeof(struct tdma_frm_req_cal));

    if ((ret = rtmac_add_header(rtdev, tdma->master_hw_addr,
                                rtskb, RTMAC_TYPE_TDMA, 0)) < 0) {
        kfree_rtskb(rtskb);
        goto err_out;
    }

    req_cal->head.version = __constant_htons(TDMA_FRM_VERSION);
    req_cal->head.id      = __constant_htons(TDMA_FRM_REQ_CAL);

    req_cal->xmit_stamp        = 0;
    req_cal->reply_cycle       = htonl(reply_cycle);
    req_cal->reply_slot_offset = cpu_to_be64(reply_slot_offset);

    rtskb->xmit_stamp = &req_cal->xmit_stamp;

    ret = rtmac_xmit(rtskb);
    if (ret < 0)
        goto err_out;

    return 0;

  err_out:
    /*ERROR*/rtdm_printk("TDMA: Failed to transmit request calibration "
                         "frame!\n");
    return ret;
}



int tdma_rt_packet_tx(struct rtskb *rtskb, struct rtnet_device *rtdev)
{
    struct tdma_priv    *tdma;
    rtdm_lockctx_t      context;
    struct tdma_slot    *slot;
    int                 ret = 0;


    tdma = (struct tdma_priv *)rtdev->mac_priv->disc_priv;

    rtcap_mark_rtmac_enqueue(rtskb);

    rtdm_lock_get_irqsave(&tdma->lock, context);

    slot = tdma->slot_table[(rtskb->priority & RTSKB_CHANNEL_MASK) >>
                            RTSKB_CHANNEL_SHIFT];

    if (unlikely(!slot)) {
        ret = -EAGAIN;
        goto err_out;
    }

    if (unlikely(rtskb->len > slot->size)) {
        ret = -EMSGSIZE;
        goto err_out;
    }

    __rtskb_prio_queue_tail(slot->queue, rtskb);

  err_out:
    rtdm_lock_put_irqrestore(&tdma->lock, context);

    return ret;
}



int tdma_nrt_packet_tx(struct rtskb *rtskb)
{
    struct tdma_priv    *tdma;
    rtdm_lockctx_t      context;
    struct tdma_slot    *slot;
    int                 ret = 0;


    tdma = (struct tdma_priv *)rtskb->rtdev->mac_priv->disc_priv;

    rtcap_mark_rtmac_enqueue(rtskb);

    rtskb->priority = RTSKB_PRIO_VALUE(QUEUE_MIN_PRIO, DEFAULT_NRT_SLOT);

    rtdm_lock_get_irqsave(&tdma->lock, context);

    slot = tdma->slot_table[DEFAULT_NRT_SLOT];

    if (unlikely(!slot)) {
        ret = -EAGAIN;
        goto err_out;
    }

    if (unlikely(rtskb->len > slot->size)) {
        ret = -EMSGSIZE;
        goto err_out;
    }

    __rtskb_prio_queue_tail(slot->queue, rtskb);

  err_out:
    rtdm_lock_put_irqrestore(&tdma->lock, context);

    return ret;
}



int tdma_packet_rx(struct rtskb *rtskb)
{
    struct tdma_priv        *tdma;
    struct tdma_frm_head    *head;
    u64                     delay;
    u64                     cycle_start;
    nanosecs_rel_t          clock_offset;
    struct rt_proc_call     *call;
    struct tdma_request_cal *req_cal_job;
    rtdm_lockctx_t          context;
#ifdef CONFIG_RTNET_TDMA_MASTER
    struct rtskb            *reply_rtskb;
    struct rtnet_device     *rtdev;
    struct tdma_frm_rpl_cal *rpl_cal_frm;
    struct tdma_reply_cal   *rpl_cal_job;
    struct tdma_job         *job;
#endif


    tdma = (struct tdma_priv *)rtskb->rtdev->mac_priv->disc_priv;

    head = (struct tdma_frm_head *)rtskb->data;

    if (head->version != __constant_htons(TDMA_FRM_VERSION))
        goto kfree_out;

    switch (head->id) {
        case __constant_htons(TDMA_FRM_SYNC):
            rtskb_pull(rtskb, sizeof(struct tdma_frm_sync));

            /* see "Time Arithmetics" in the TDMA specification */
            clock_offset = be64_to_cpu(SYNC_FRM(head)->xmit_stamp) +
                    tdma->master_packet_delay_ns;
            clock_offset -= rtskb->time_stamp;

            cycle_start = be64_to_cpu(SYNC_FRM(head)->sched_xmit_stamp) -
                    clock_offset;

            rtdm_lock_get_irqsave(&tdma->lock, context);
            tdma->current_cycle       = ntohl(SYNC_FRM(head)->cycle_no);
            tdma->current_cycle_start = cycle_start;
            tdma->clock_offset        = clock_offset;
            rtdm_lock_put_irqrestore(&tdma->lock, context);

            /* note: Ethernet-specific! */
            memcpy(tdma->master_hw_addr, rtskb->mac.ethernet->h_source,
                   ETH_ALEN);

            set_bit(TDMA_FLAG_RECEIVED_SYNC, &tdma->flags);

            rtdm_event_pulse(&tdma->sync_event);
            break;

#ifdef CONFIG_RTNET_TDMA_MASTER
        case __constant_htons(TDMA_FRM_REQ_CAL):
            RTNET_ASSERT(test_bit(TDMA_FLAG_MASTER, &tdma->flags) &&
                         test_bit(TDMA_FLAG_CALIBRATED, &tdma->flags),
                         break;);

            rtskb_pull(rtskb, sizeof(struct tdma_frm_req_cal));

            rtdev = rtskb->rtdev;

            reply_rtskb = alloc_rtskb(rtdev->hard_header_len +
                                      sizeof(struct rtmac_hdr) +
                                      sizeof(struct tdma_frm_rpl_cal) + 15,
                                      &tdma->cal_rtskb_pool);
            if (unlikely(!reply_rtskb)) {
                /*ERROR*/rtdm_printk("TDMA: Too many calibration requests "
                                     "pending!\n");
                break;
            }

            rtskb_reserve(reply_rtskb, (rtdev->hard_header_len +
                          sizeof(struct rtmac_hdr) + 15) & ~15);

            rpl_cal_frm = (struct tdma_frm_rpl_cal *)
                rtskb_put(reply_rtskb, sizeof(struct tdma_frm_rpl_cal));

            /* note: Ethernet-specific! */
            if (unlikely(rtmac_add_header(rtdev, rtskb->mac.ethernet->h_source,
                                          reply_rtskb, RTMAC_TYPE_TDMA,
                                          0) < 0)) {
                kfree_rtskb(reply_rtskb);
                break;
            }

            rpl_cal_frm->head.version = __constant_htons(TDMA_FRM_VERSION);
            rpl_cal_frm->head.id      = __constant_htons(TDMA_FRM_RPL_CAL);

            rpl_cal_frm->request_xmit_stamp = REQ_CAL_FRM(head)->xmit_stamp;
            rpl_cal_frm->reception_stamp    = cpu_to_be64(rtskb->time_stamp);
            rpl_cal_frm->xmit_stamp         = 0;

            reply_rtskb->xmit_stamp = &rpl_cal_frm->xmit_stamp;

            /* use reply_rtskb memory behind the frame as job buffer */
            rpl_cal_job = (struct tdma_reply_cal *)reply_rtskb->tail;
            RTNET_ASSERT(reply_rtskb->tail +
                sizeof(struct tdma_reply_cal) <= reply_rtskb->buf_end,
                rtskb_over_panic(reply_rtskb, sizeof(struct tdma_reply_cal),
                                 current_text_addr()););

            rpl_cal_job->head.id        = XMIT_RPL_CAL;
            rpl_cal_job->head.ref_count = 0;
            rpl_cal_job->reply_cycle    =
                    ntohl(REQ_CAL_FRM(head)->reply_cycle);
            rpl_cal_job->reply_rtskb    = reply_rtskb;
            rpl_cal_job->reply_offset   =
                    be64_to_cpu(REQ_CAL_FRM(head)->reply_slot_offset);

            rtdm_lock_get_irqsave(&tdma->lock, context);

            job = tdma->current_job;
            while (1) {
                job = list_entry(job->entry.prev, struct tdma_job, entry);
                if ((job == tdma->first_job) ||
                    ((job->id >= 0) &&
                     (SLOT_JOB(job)->offset < rpl_cal_job->reply_offset)) ||
                    ((job->id == XMIT_RPL_CAL) &&
                     (REPLY_CAL_JOB(job)->reply_offset <
                            rpl_cal_job->reply_offset)))
                    break;
            }
            list_add(&rpl_cal_job->head.entry, &job->entry);
            tdma->job_list_revision++;

            rtdm_lock_put_irqrestore(&tdma->lock, context);

            break;
#endif

        case __constant_htons(TDMA_FRM_RPL_CAL):
            rtskb_pull(rtskb, sizeof(struct tdma_frm_rpl_cal));

            /* see "Time Arithmetics" in the TDMA specification */
            delay = (rtskb->time_stamp -
                     be64_to_cpu(RPL_CAL_FRM(head)->request_xmit_stamp)) -
                    (be64_to_cpu(RPL_CAL_FRM(head)->xmit_stamp) -
                     be64_to_cpu(RPL_CAL_FRM(head)->reception_stamp));
            delay = (delay + 1) >> 1;

            rtdm_lock_get_irqsave(&tdma->lock, context);

            call = tdma->calibration_call;
            if (call == NULL) {
                rtdm_lock_put_irqrestore(&tdma->lock, context);
                break;
            }
            req_cal_job = rtpc_get_priv(call, struct tdma_request_cal);

            req_cal_job->result_buffer[--req_cal_job->cal_rounds] = delay;

            if (req_cal_job->cal_rounds > 0) {
                tdma->job_list_revision++;
                list_add(&req_cal_job->head.entry, &tdma->first_job->entry);

                rtdm_lock_put_irqrestore(&tdma->lock, context);

            } else {
                tdma->calibration_call = NULL;

                rtdm_lock_put_irqrestore(&tdma->lock, context);

                rtpc_complete_call(call, 0);
            }

            break;

        default:
            /*ERROR*/rtdm_printk("TDMA: Unknown frame %d!\n", ntohs(head->id));
    }

  kfree_out:
    kfree_rtskb(rtskb);
    return 0;
}



unsigned int tdma_get_mtu(struct rtnet_device *rtdev, unsigned int priority)
{
    struct tdma_priv    *tdma;
    rtdm_lockctx_t      context;
    struct tdma_slot    *slot;
    unsigned int        mtu;


    tdma = (struct tdma_priv *)rtdev->mac_priv->disc_priv;

    rtdm_lock_get_irqsave(&tdma->lock, context);

    slot = tdma->slot_table[(priority & RTSKB_CHANNEL_MASK) >>
                            RTSKB_CHANNEL_SHIFT];

    if (unlikely(!slot)) {
        mtu = rtdev->mtu;
        goto out;
    }

    mtu = slot->mtu;

  out:
    rtdm_lock_put_irqrestore(&tdma->lock, context);

    return mtu;
}