/***
|
*
|
* rtmac/tdma/tdma_proto.c
|
*
|
* RTmac - real-time networking media access control subsystem
|
* Copyright (C) 2002 Marc Kleine-Budde <kleine-budde@gmx.de>,
|
* 2003-2005 Jan Kiszka <Jan.Kiszka@web.de>
|
*
|
* 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_XENO_DRIVERS_NET_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_XENO_DRIVERS_NET_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;
|
}
|
