/***
 *
 *  rtcfg/rtcfg_frame.c
 *
 *  Real-Time Configuration Distribution Protocol
 *
 *  Copyright (C) 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/moduleparam.h>
#include <linux/if_ether.h>

#include <stack_mgr.h>
#include <rtcfg/rtcfg.h>
#include <rtcfg/rtcfg_conn_event.h>
#include <rtcfg/rtcfg_frame.h>
#include <rtcfg/rtcfg_timer.h>

static unsigned int num_rtskbs = 32;
module_param(num_rtskbs, uint, 0444);
MODULE_PARM_DESC(num_rtskbs, "Number of realtime socket buffers used by RTcfg");

static struct rtskb_pool rtcfg_pool;
static rtdm_task_t rx_task;
static rtdm_event_t rx_event;
static struct rtskb_queue rx_queue;

void rtcfg_thread_signal(void)
{
	rtdm_event_signal(&rx_event);
}

static int rtcfg_rx_handler(struct rtskb *rtskb, struct rtpacket_type *pt)
{
	if (rtskb_acquire(rtskb, &rtcfg_pool) == 0) {
		rtskb_queue_tail(&rx_queue, rtskb);
		rtcfg_thread_signal();
	} else
		kfree_rtskb(rtskb);

	return 0;
}

static void rtcfg_rx_task(void *arg)
{
	struct rtskb *rtskb;
	struct rtcfg_frm_head *frm_head;
	struct rtnet_device *rtdev;

	while (!rtdm_task_should_stop()) {
		if (rtdm_event_wait(&rx_event) < 0)
			break;

		while ((rtskb = rtskb_dequeue(&rx_queue))) {
			rtdev = rtskb->rtdev;

			if (rtskb->pkt_type == PACKET_OTHERHOST) {
				kfree_rtskb(rtskb);
				continue;
			}

			if (rtskb->len < sizeof(struct rtcfg_frm_head)) {
				RTCFG_DEBUG(
					1,
					"RTcfg: %s() received an invalid frame\n",
					__FUNCTION__);
				kfree_rtskb(rtskb);
				continue;
			}

			frm_head = (struct rtcfg_frm_head *)rtskb->data;

			if (rtcfg_do_main_event(rtskb->rtdev->ifindex,
						frm_head->id +
							RTCFG_FRM_STAGE_1_CFG,
						rtskb) < 0)
				kfree_rtskb(rtskb);
		}

		rtcfg_timer_run();
	}
}

int rtcfg_send_frame(struct rtskb *rtskb, struct rtnet_device *rtdev,
		     u8 *dest_addr)
{
	int ret;

	rtskb->rtdev = rtdev;
	rtskb->priority = RTCFG_SKB_PRIO;

	if (rtdev->hard_header) {
		ret = rtdev->hard_header(rtskb, rtdev, ETH_RTCFG, dest_addr,
					 rtdev->dev_addr, rtskb->len);
		if (ret < 0)
			goto err;
	}

	if ((rtdev->flags & IFF_UP) != 0) {
		ret = 0;
		if (rtdev_xmit(rtskb) != 0)
			ret = -EAGAIN;
	} else {
		ret = -ENETDOWN;
		goto err;
	}

	rtdev_dereference(rtdev);
	return ret;

err:
	kfree_rtskb(rtskb);
	rtdev_dereference(rtdev);
	return ret;
}

int rtcfg_send_stage_1(struct rtcfg_connection *conn)
{
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_stage_1_cfg *stage_1_frm;

	rtdev = rtdev_get_by_index(conn->ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	rtskb_size = rtdev->hard_header_len +
		     sizeof(struct rtcfg_frm_stage_1_cfg) + conn->stage1_size +
#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
		     (((conn->addr_type & RTCFG_ADDR_MASK) == RTCFG_ADDR_IP) ?
			      2 * RTCFG_ADDRSIZE_IP :
			      0);
#else /* !CONFIG_XENO_DRIVERS_NET_RTIPV4 */
		     0;
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	stage_1_frm = (struct rtcfg_frm_stage_1_cfg *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_stage_1_cfg));

	stage_1_frm->head.id = RTCFG_ID_STAGE_1_CFG;
	stage_1_frm->head.version = 0;
	stage_1_frm->addr_type = conn->addr_type & RTCFG_ADDR_MASK;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
	if (stage_1_frm->addr_type == RTCFG_ADDR_IP) {
		rtskb_put(rtskb, 2 * RTCFG_ADDRSIZE_IP);

		memcpy(stage_1_frm->client_addr, &(conn->addr.ip_addr), 4);

		stage_1_frm =
			(struct rtcfg_frm_stage_1_cfg *)(((u8 *)stage_1_frm) +
							 RTCFG_ADDRSIZE_IP);

		memcpy(stage_1_frm->server_addr, &(rtdev->local_ip), 4);

		stage_1_frm =
			(struct rtcfg_frm_stage_1_cfg *)(((u8 *)stage_1_frm) +
							 RTCFG_ADDRSIZE_IP);
	}
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	stage_1_frm->burstrate = device[conn->ifindex].burstrate;
	stage_1_frm->cfg_len = htons(conn->stage1_size);

	memcpy(rtskb_put(rtskb, conn->stage1_size), conn->stage1_data,
	       conn->stage1_size);

	return rtcfg_send_frame(rtskb, rtdev, conn->mac_addr);
}

int rtcfg_send_stage_2(struct rtcfg_connection *conn, int send_data)
{
	struct rtnet_device *rtdev;
	struct rtcfg_device *rtcfg_dev = &device[conn->ifindex];
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_stage_2_cfg *stage_2_frm;
	size_t total_size;
	size_t frag_size;

	rtdev = rtdev_get_by_index(conn->ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	if (send_data) {
		total_size = conn->stage2_file->size;
		frag_size = MIN(rtdev->get_mtu(rtdev, RTCFG_SKB_PRIO) -
					sizeof(struct rtcfg_frm_stage_2_cfg),
				total_size);
	} else {
		total_size = 0;
		frag_size = 0;
	}

	rtskb_size = rtdev->hard_header_len +
		     sizeof(struct rtcfg_frm_stage_2_cfg) + frag_size;

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	stage_2_frm = (struct rtcfg_frm_stage_2_cfg *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_stage_2_cfg));

	stage_2_frm->head.id = RTCFG_ID_STAGE_2_CFG;
	stage_2_frm->head.version = 0;
	stage_2_frm->flags = rtcfg_dev->flags;
	stage_2_frm->stations = htonl(rtcfg_dev->other_stations);
	stage_2_frm->heartbeat_period = htons(rtcfg_dev->spec.srv.heartbeat);
	stage_2_frm->cfg_len = htonl(total_size);

	if (send_data)
		memcpy(rtskb_put(rtskb, frag_size), conn->stage2_file->buffer,
		       frag_size);
	conn->cfg_offs = frag_size;

	return rtcfg_send_frame(rtskb, rtdev, conn->mac_addr);
}

int rtcfg_send_stage_2_frag(struct rtcfg_connection *conn)
{
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_stage_2_cfg_frag *stage_2_frm;
	size_t frag_size;

	rtdev = rtdev_get_by_index(conn->ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	frag_size = MIN(rtdev->get_mtu(rtdev, RTCFG_SKB_PRIO) -
				sizeof(struct rtcfg_frm_stage_2_cfg_frag),
			conn->stage2_file->size - conn->cfg_offs);

	rtskb_size = rtdev->hard_header_len +
		     sizeof(struct rtcfg_frm_stage_2_cfg_frag) + frag_size;

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	stage_2_frm = (struct rtcfg_frm_stage_2_cfg_frag *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_stage_2_cfg_frag));

	stage_2_frm->head.id = RTCFG_ID_STAGE_2_CFG_FRAG;
	stage_2_frm->head.version = 0;
	stage_2_frm->frag_offs = htonl(conn->cfg_offs);

	memcpy(rtskb_put(rtskb, frag_size),
	       conn->stage2_file->buffer + conn->cfg_offs, frag_size);
	conn->cfg_offs += frag_size;

	return rtcfg_send_frame(rtskb, rtdev, conn->mac_addr);
}

int rtcfg_send_announce_new(int ifindex)
{
	struct rtcfg_device *rtcfg_dev = &device[ifindex];
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_announce *announce_new;

	rtdev = rtdev_get_by_index(ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	rtskb_size = rtdev->hard_header_len +
		     sizeof(struct rtcfg_frm_announce) +
#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
		     (((rtcfg_dev->spec.clt.addr_type & RTCFG_ADDR_MASK) ==
		       RTCFG_ADDR_IP) ?
			      RTCFG_ADDRSIZE_IP :
			      0);
#else /* !CONFIG_XENO_DRIVERS_NET_RTIPV4 */
		     0;
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	announce_new = (struct rtcfg_frm_announce *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_announce));

	announce_new->head.id = RTCFG_ID_ANNOUNCE_NEW;
	announce_new->head.version = 0;
	announce_new->addr_type = rtcfg_dev->spec.clt.addr_type;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
	if (announce_new->addr_type == RTCFG_ADDR_IP) {
		rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);

		memcpy(announce_new->addr, &(rtdev->local_ip), 4);

		announce_new =
			(struct rtcfg_frm_announce *)(((u8 *)announce_new) +
						      RTCFG_ADDRSIZE_IP);
	}
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	announce_new->flags = rtcfg_dev->flags;
	announce_new->burstrate = rtcfg_dev->burstrate;

	return rtcfg_send_frame(rtskb, rtdev, rtdev->broadcast);
}

int rtcfg_send_announce_reply(int ifindex, u8 *dest_mac_addr)
{
	struct rtcfg_device *rtcfg_dev = &device[ifindex];
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_announce *announce_rpl;

	rtdev = rtdev_get_by_index(ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	rtskb_size = rtdev->hard_header_len +
		     sizeof(struct rtcfg_frm_announce) +
#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
		     ((rtcfg_dev->spec.clt.addr_type == RTCFG_ADDR_IP) ?
			      RTCFG_ADDRSIZE_IP :
			      0);
#else /* !CONFIG_XENO_DRIVERS_NET_RTIPV4 */
		     0;
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	announce_rpl = (struct rtcfg_frm_announce *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_announce));

	announce_rpl->head.id = RTCFG_ID_ANNOUNCE_REPLY;
	announce_rpl->head.version = 0;
	announce_rpl->addr_type = rtcfg_dev->spec.clt.addr_type;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
	if (announce_rpl->addr_type == RTCFG_ADDR_IP) {
		rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);

		memcpy(announce_rpl->addr, &(rtdev->local_ip), 4);

		announce_rpl =
			(struct rtcfg_frm_announce *)(((u8 *)announce_rpl) +
						      RTCFG_ADDRSIZE_IP);
	}
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	announce_rpl->flags = rtcfg_dev->flags & _RTCFG_FLAG_READY;
	announce_rpl->burstrate = 0; /* padding field */

	return rtcfg_send_frame(rtskb, rtdev, dest_mac_addr);
}

int rtcfg_send_ack(int ifindex)
{
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_ack_cfg *ack_frm;

	rtdev = rtdev_get_by_index(ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	rtskb_size = rtdev->hard_header_len + sizeof(struct rtcfg_frm_ack_cfg);

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	ack_frm = (struct rtcfg_frm_ack_cfg *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_ack_cfg));

	ack_frm->head.id = RTCFG_ID_ACK_CFG;
	ack_frm->head.version = 0;
	ack_frm->ack_len = htonl(device[ifindex].spec.clt.cfg_offs);

	return rtcfg_send_frame(rtskb, rtdev,
				device[ifindex].spec.clt.srv_mac_addr);
}

int rtcfg_send_simple_frame(int ifindex, int frame_id, u8 *dest_addr)
{
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_simple *simple_frm;

	rtdev = rtdev_get_by_index(ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	rtskb_size = rtdev->hard_header_len + sizeof(struct rtcfg_frm_simple);

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	simple_frm = (struct rtcfg_frm_simple *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_simple));

	simple_frm->head.id = frame_id;
	simple_frm->head.version = 0;

	return rtcfg_send_frame(rtskb, rtdev,
				(dest_addr) ? dest_addr : rtdev->broadcast);
}

int rtcfg_send_dead_station(struct rtcfg_connection *conn)
{
	struct rtnet_device *rtdev;
	struct rtskb *rtskb;
	unsigned int rtskb_size;
	struct rtcfg_frm_dead_station *dead_station_frm;

	rtdev = rtdev_get_by_index(conn->ifindex);
	if (rtdev == NULL)
		return -ENODEV;

	rtskb_size = rtdev->hard_header_len +
		     sizeof(struct rtcfg_frm_dead_station) +
#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
		     (((conn->addr_type & RTCFG_ADDR_MASK) == RTCFG_ADDR_IP) ?
			      RTCFG_ADDRSIZE_IP :
			      0);
#else /* !CONFIG_XENO_DRIVERS_NET_RTIPV4 */
		     0;
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
	if (rtskb == NULL) {
		rtdev_dereference(rtdev);
		return -ENOBUFS;
	}

	rtskb_reserve(rtskb, rtdev->hard_header_len);

	dead_station_frm = (struct rtcfg_frm_dead_station *)rtskb_put(
		rtskb, sizeof(struct rtcfg_frm_dead_station));

	dead_station_frm->head.id = RTCFG_ID_DEAD_STATION;
	dead_station_frm->head.version = 0;
	dead_station_frm->addr_type = conn->addr_type & RTCFG_ADDR_MASK;

#if IS_ENABLED(CONFIG_XENO_DRIVERS_NET_RTIPV4)
	if (dead_station_frm->addr_type == RTCFG_ADDR_IP) {
		rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);

		memcpy(dead_station_frm->logical_addr, &(conn->addr.ip_addr),
		       4);

		dead_station_frm = (struct rtcfg_frm_dead_station
					    *)(((u8 *)dead_station_frm) +
					       RTCFG_ADDRSIZE_IP);
	}
#endif /* CONFIG_XENO_DRIVERS_NET_RTIPV4 */

	/* Ethernet-specific! */
	memcpy(dead_station_frm->physical_addr, conn->mac_addr, ETH_ALEN);
	memset(&dead_station_frm->physical_addr[ETH_ALEN], 0,
	       sizeof(dead_station_frm->physical_addr) - ETH_ALEN);

	return rtcfg_send_frame(rtskb, rtdev, rtdev->broadcast);
}

static struct rtpacket_type rtcfg_packet_type = { .type = __constant_htons(
							  ETH_RTCFG),
						  .handler = rtcfg_rx_handler };

int __init rtcfg_init_frames(void)
{
	int ret;

	if (rtskb_module_pool_init(&rtcfg_pool, num_rtskbs) < num_rtskbs)
		return -ENOMEM;

	rtskb_queue_init(&rx_queue);
	rtdm_event_init(&rx_event, 0);

	ret = rtdm_task_init(&rx_task, "rtcfg-rx", rtcfg_rx_task, 0,
			     RTDM_TASK_LOWEST_PRIORITY, 0);
	if (ret < 0) {
		rtdm_event_destroy(&rx_event);
		goto error1;
	}

	ret = rtdev_add_pack(&rtcfg_packet_type);
	if (ret < 0)
		goto error2;

	return 0;

error2:
	rtdm_event_destroy(&rx_event);
	rtdm_task_destroy(&rx_task);

error1:
	rtskb_pool_release(&rtcfg_pool);

	return ret;
}

void rtcfg_cleanup_frames(void)
{
	struct rtskb *rtskb;

	rtdev_remove_pack(&rtcfg_packet_type);

	rtdm_event_destroy(&rx_event);
	rtdm_task_destroy(&rx_task);

	while ((rtskb = rtskb_dequeue(&rx_queue)) != NULL) {
		kfree_rtskb(rtskb);
	}

	rtskb_pool_release(&rtcfg_pool);
}