/***
 *
 *  rtmac/tdma/tdma_ioctl.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/module.h>
#include <linux/delay.h>
#include <linux/uaccess.h>
#include <asm/div64.h>

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

#ifdef CONFIG_XENO_DRIVERS_NET_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_module_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_XENO_DRIVERS_NET_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);
		if (!req_cal.result_buffer) {
			kfree(slot);
			return -ENOMEM;
		}

		ret = rtpc_dispatch_call(start_calibration, 0, &req_cal,
					 sizeof(req_cal), copyback_calibration,
					 cleanup_calibration);
		if (ret < 0) {
			/* kick out any pending calibration job before returning */
			rtdm_lock_get_irqsave(&tdma->lock, context);

			job = list_entry(tdma->first_job->entry.next,
					 struct tdma_job, entry);
			if (job != tdma->first_job) {
				__list_del(job->entry.prev, job->entry.next);

				while (job->ref_count > 0) {
					rtdm_lock_put_irqrestore(&tdma->lock,
								 context);
					msleep(100);
					rtdm_lock_get_irqsave(&tdma->lock,
							      context);
				}
			}

			rtdm_lock_put_irqrestore(&tdma->lock, context);

			kfree(slot);
			return ret;
		}

#ifdef CONFIG_XENO_DRIVERS_NET_TDMA_MASTER
		if (test_bit(TDMA_FLAG_MASTER, &tdma->flags)) {
			u32 cycle_no = (volatile u32)tdma->current_cycle;
			u64 cycle_ms;

			/* switch back to [backup] master mode */
			if (test_bit(TDMA_FLAG_BACKUP_MASTER, &tdma->flags))
				tdma->sync_job.id = BACKUP_SYNC;
			else
				tdma->sync_job.id = XMIT_SYNC;

			/* wait 2 cycle periods for the mode switch */
			cycle_ms = tdma->cycle_period;
			do_div(cycle_ms, 1000000);
			if (cycle_ms == 0)
				cycle_ms = 1;
			msleep(2 * cycle_ms);

			/* catch the very unlikely case that the current master died
               while we just switched the mode */
			if (cycle_no == (volatile u32)tdma->current_cycle) {
				kfree(slot);
				return -ETIME;
			}
		}
#endif /* CONFIG_XENO_DRIVERS_NET_TDMA_MASTER */

		set_bit(TDMA_FLAG_CALIBRATED, &tdma->flags);
	}

	slot->head.id = id;
	slot->head.ref_count = 0;
	slot->period = cfg->args.set_slot.period;
	slot->phasing = cfg->args.set_slot.phasing;
	slot->mtu = cfg->args.set_slot.size;
	slot->size = cfg->args.set_slot.size + rtdev->hard_header_len;
	slot->offset = cfg->args.set_slot.offset;
	slot->queue = &slot->local_queue;
	rtskb_prio_queue_init(&slot->local_queue);

	if (jnt_id >= 0) /* all other validation tests performed above */
		slot->queue = tdma->slot_table[jnt_id]->queue;

	old_slot = tdma->slot_table[id];
	if ((id == DEFAULT_NRT_SLOT) &&
	    (old_slot == tdma->slot_table[DEFAULT_SLOT]))
		old_slot = NULL;

restart:
	job_list_revision = tdma->job_list_revision;

	if (!old_slot) {
		job = tdma->first_job;
		while (1) {
			prev_job = job;
			job = list_entry(job->entry.next, struct tdma_job,
					 entry);
			if (((job->id >= 0) &&
			     ((slot->offset < SLOT_JOB(job)->offset) ||
			      ((slot->offset == SLOT_JOB(job)->offset) &&
			       (slot->head.id <= SLOT_JOB(job)->head.id)))) ||
#ifdef CONFIG_XENO_DRIVERS_NET_TDMA_MASTER
			    ((job->id == XMIT_RPL_CAL) &&
			     (slot->offset <
			      REPLY_CAL_JOB(job)->reply_offset)) ||
#endif /* CONFIG_XENO_DRIVERS_NET_TDMA_MASTER */
			    (job == tdma->first_job))
				break;
		}

	} else
		prev_job = list_entry(old_slot->head.entry.prev,
				      struct tdma_job, entry);

	rtdm_lock_get_irqsave(&tdma->lock, context);

	if (job_list_revision != tdma->job_list_revision) {
		rtdm_lock_put_irqrestore(&tdma->lock, context);

		msleep(100);
		goto restart;
	}

	if (old_slot)
		__list_del(old_slot->head.entry.prev,
			   old_slot->head.entry.next);

	list_add(&slot->head.entry, &prev_job->entry);
	tdma->slot_table[id] = slot;
	if ((id == DEFAULT_SLOT) &&
	    (tdma->slot_table[DEFAULT_NRT_SLOT] == old_slot))
		tdma->slot_table[DEFAULT_NRT_SLOT] = slot;

	if (old_slot) {
		while (old_slot->head.ref_count > 0) {
			rtdm_lock_put_irqrestore(&tdma->lock, context);
			msleep(100);
			rtdm_lock_get_irqsave(&tdma->lock, context);
		}

		rtdm_lock_put_irqrestore(&tdma->lock, context);

		/* search for other slots linked to the old one */
		for (jnt_id = 0; jnt_id < tdma->max_slot_id; jnt_id++)
			if ((tdma->slot_table[jnt_id] != 0) &&
			    (tdma->slot_table[jnt_id]->queue ==
			     &old_slot->local_queue)) {
				/* found a joint slot, move or detach it now */
				rtdm_lock_get_irqsave(&tdma->lock, context);

				while (tdma->slot_table[jnt_id]->head.ref_count >
				       0) {
					rtdm_lock_put_irqrestore(&tdma->lock,
								 context);
					msleep(100);
					rtdm_lock_get_irqsave(&tdma->lock,
							      context);
				}

				/* If the new slot size is larger, detach the other slot,
                 * update it otherwise. */
				if (slot->mtu > tdma->slot_table[jnt_id]->mtu)
					tdma->slot_table[jnt_id]->queue =
						&tdma->slot_table[jnt_id]
							 ->local_queue;
				else {
					tdma->slot_table[jnt_id]->mtu =
						slot->mtu;
					tdma->slot_table[jnt_id]->queue =
						slot->queue;
				}

				rtdm_lock_put_irqrestore(&tdma->lock, context);
			}
	} else
		rtdm_lock_put_irqrestore(&tdma->lock, context);

	rtmac_vnic_set_max_mtu(rtdev, cfg->args.set_slot.size);

	if (old_slot) {
		/* avoid that the formerly joint queue gets purged */
		old_slot->queue = &old_slot->local_queue;

		/* Without any reference to the old job and no joint slots we can
         * safely purge its queue without lock protection.
         * NOTE: Reconfiguring a slot during runtime may lead to packet
         *       drops! */
		while ((rtskb = __rtskb_prio_dequeue(old_slot->queue)))
			kfree_rtskb(rtskb);

		kfree(old_slot);
	}

	return 0;
}

int tdma_cleanup_slot(struct tdma_priv *tdma, struct tdma_slot *slot)
{
	struct rtskb *rtskb;
	unsigned int id, jnt_id;
	rtdm_lockctx_t context;

	if (!slot)
		return -EINVAL;

	id = slot->head.id;

	rtdm_lock_get_irqsave(&tdma->lock, context);

	__list_del(slot->head.entry.prev, slot->head.entry.next);

	if (id == DEFAULT_NRT_SLOT)
		tdma->slot_table[DEFAULT_NRT_SLOT] =
			tdma->slot_table[DEFAULT_SLOT];
	else {
		if ((id == DEFAULT_SLOT) &&
		    (tdma->slot_table[DEFAULT_NRT_SLOT] == slot))
			tdma->slot_table[DEFAULT_NRT_SLOT] = NULL;
		tdma->slot_table[id] = NULL;
	}

	while (slot->head.ref_count > 0) {
		rtdm_lock_put_irqrestore(&tdma->lock, context);
		msleep(100);
		rtdm_lock_get_irqsave(&tdma->lock, context);
	}

	rtdm_lock_put_irqrestore(&tdma->lock, context);

	/* search for other slots linked to this one */
	for (jnt_id = 0; jnt_id < tdma->max_slot_id; jnt_id++)
		if ((tdma->slot_table[jnt_id] != 0) &&
		    (tdma->slot_table[jnt_id]->queue == &slot->local_queue)) {
			/* found a joint slot, detach it now under lock protection */
			rtdm_lock_get_irqsave(&tdma->lock, context);

			while (tdma->slot_table[jnt_id]->head.ref_count > 0) {
				rtdm_lock_put_irqrestore(&tdma->lock, context);
				msleep(100);
				rtdm_lock_get_irqsave(&tdma->lock, context);
			}
			tdma->slot_table[jnt_id]->queue =
				&tdma->slot_table[jnt_id]->local_queue;

			rtdm_lock_put_irqrestore(&tdma->lock, context);
		}

	/* avoid that the formerly joint queue gets purged */
	slot->queue = &slot->local_queue;

	/* No need to protect the queue access here -
     * no one is referring to this job anymore
     * (ref_count == 0, all joint slots detached). */
	while ((rtskb = __rtskb_prio_dequeue(slot->queue)))
		kfree_rtskb(rtskb);

	kfree(slot);

	return 0;
}

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

	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.remove_slot.id;
	if (id > tdma->max_slot_id)
		return -EINVAL;

	if ((id == DEFAULT_NRT_SLOT) && (tdma->slot_table[DEFAULT_NRT_SLOT] ==
					 tdma->slot_table[DEFAULT_SLOT]))
		return -EINVAL;

	return tdma_cleanup_slot(tdma, tdma->slot_table[id]);
}

static int tdma_ioctl_detach(struct rtnet_device *rtdev)
{
	struct tdma_priv *tdma;
	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;

	ret = rtmac_disc_detach(rtdev);

	return ret;
}

int tdma_ioctl(struct rtnet_device *rtdev, unsigned int request,
	       unsigned long arg)
{
	struct tdma_config cfg;
	int ret;

	ret = copy_from_user(&cfg, (void *)arg, sizeof(cfg));
	if (ret != 0)
		return -EFAULT;

	if (mutex_lock_interruptible(&rtdev->nrt_lock))
		return -ERESTARTSYS;

	switch (request) {
#ifdef CONFIG_XENO_DRIVERS_NET_TDMA_MASTER
	case TDMA_IOC_MASTER:
		ret = tdma_ioctl_master(rtdev, &cfg);
		break;
#endif
	case TDMA_IOC_SLAVE:
		ret = tdma_ioctl_slave(rtdev, &cfg);
		break;

	case TDMA_IOC_CAL_RESULT_SIZE:
		ret = tdma_ioctl_cal_result_size(rtdev, &cfg);
		break;

	case TDMA_IOC_SET_SLOT:
		ret = tdma_ioctl_set_slot(rtdev, &cfg);
		break;

	case TDMA_IOC_REMOVE_SLOT:
		ret = tdma_ioctl_remove_slot(rtdev, &cfg);
		break;

	case TDMA_IOC_DETACH:
		ret = tdma_ioctl_detach(rtdev);
		break;

	default:
		ret = -ENOTTY;
	}

	mutex_unlock(&rtdev->nrt_lock);

	return ret;
}