/*
 * Copyright (C) 2005, 2006 Sebastian Smolorz
 *                          <Sebastian.Smolorz@stud.uni-hannover.de>
 *
 * Copyright (C) 2006 Wolfgang Grandegger <wg@grandegger.com>
 *
 *
 * 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; eitherer 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <linux/module.h>
#include <linux/delay.h>

#include <rtdm/driver.h>

#include <rtdm/can.h>
#include "rtcan_internal.h"
#include "rtcan_socket.h"
#include "rtcan_list.h"
#include "rtcan_dev.h"
#include "rtcan_raw.h"


#if 0
void rtcan_raw_print_filter(struct rtcan_device *dev)
{
    int i;
    struct rtcan_recv *r = dev->receivers;

    rtdm_printk("%s: recv_list=%p empty_list=%p free_entries=%d\n",
		dev->name, dev->recv_list, dev->empty_list, dev->free_entries);
    for (i = 0; i < RTCAN_MAX_RECEIVERS; i++, r++) {
	rtdm_printk("%2d %p sock=%p next=%p id=%x mask=%x\n",
		    i, r, r->sock, r->next,
		    r->can_filter.can_id, r->can_filter.can_mask);
    }
}
#else
#define rtcan_raw_print_filter(dev)
#endif


static inline void rtcan_raw_mount_filter(can_filter_t *recv_filter,
					  can_filter_t *filter)
{
    if (filter->can_id & CAN_INV_FILTER) {
	recv_filter->can_id = filter->can_id & ~CAN_INV_FILTER;
	recv_filter->can_mask = filter->can_mask | CAN_INV_FILTER;
    } else {
	recv_filter->can_id = filter->can_id;
	recv_filter->can_mask = filter->can_mask & ~CAN_INV_FILTER;
    }

    /* Apply mask for fast filter check */
    recv_filter->can_id &= recv_filter->can_mask;
}


int rtcan_raw_check_filter(struct rtcan_socket *sock, int ifindex,
			   struct rtcan_filter_list *flist)
{
    int old_ifindex = 0, old_flistlen_all = 0;
    int free_entries, i, begin, end;
    struct rtcan_device *dev;
    int flistlen;

    if (rtcan_flist_no_filter(flist))
	return 0;

    /* Check if filter list has been defined by user */
    flistlen = (flist) ? flist->flistlen : 1;

    /* Now we check if a reception list would overflow. This takes some
     * preparation, so let's go ... */

    /* Check current bind status */
    if (rtcan_sock_has_filter(sock)) {
	/* Socket is bound */
	i = atomic_read(&sock->ifindex);

	if (i == 0)
	    /* Socket was bound to ALL interfaces */
	    old_flistlen_all = sock->flistlen;
	else    /* Socket was bound to only one interface */
	    old_ifindex = i;
    }

    if (ifindex) {
	/* We bind the socket to only one interface. */
	begin = ifindex;
	end   = ifindex;
    } else {
	/* Socket must be bound to all interfaces. */
	begin = 1;
	end = RTCAN_MAX_DEVICES;
    }

    /* Check if there is space for the new binding */
    for (i = begin; i <= end; i++) {
	if ((dev = rtcan_dev_get_by_index(i)) == NULL)
	    continue;
	free_entries = dev->free_entries + old_flistlen_all;
	rtcan_dev_dereference(dev);
	if (i == old_ifindex)
	    free_entries += sock->flistlen;
	/* Compare free list space to new filter list length */
	if (free_entries < flistlen)
	    return -ENOSPC;
    }

    return 0;
}


int rtcan_raw_add_filter(struct rtcan_socket *sock, int ifindex)
{
    int i, j, begin, end;
    struct rtcan_recv *first, *last;
    struct rtcan_device *dev;
    /* Check if filter list has been defined by user */
    int flistlen;

    if (rtcan_flist_no_filter(sock->flist)) {
	return 0;
    }

    flistlen = (sock->flist) ? sock->flist->flistlen : 0;

    if (ifindex) {
	/* We bind the socket to only one interface. */
	begin = ifindex;
	end   = ifindex;
    } else {
	/* Socket must be bound to all interfaces. */
	begin = 1;
	end = RTCAN_MAX_DEVICES;
    }

    for (i = begin; i <= end; i++) {
	if ((dev = rtcan_dev_get_by_index(i)) == NULL)
	    continue;

	/* Take first entry of empty list */
	first = last = dev->empty_list;
	/* Check if filter list is empty */
	if (flistlen) {
	    /* Filter list is not empty */
	    /* Register first filter */
	    rtcan_raw_mount_filter(&last->can_filter,
				   &sock->flist->flist[0]);
	    last->match_count = 0;
	    last->sock = sock;
	    for (j = 1; j < flistlen; j++) {
		/* Register remaining filters */
		last = last->next;
		rtcan_raw_mount_filter(&last->can_filter,
				       &sock->flist->flist[j]);
		last->sock = sock;
		last->match_count = 0;
	    }
	    /* Decrease free entries counter by length of filter list */
	    dev->free_entries -= flistlen;

	} else {
	    /* Filter list is empty. Socket must be bound to all CAN IDs. */
	    /* Fill list entry members */
	    last->can_filter.can_id = last->can_filter.can_mask = 0;
	    last->sock = sock;
	    last->match_count = 0;
	    /* Decrease free entries counter by 1
	     * (one filter for all CAN frames) */
	    dev->free_entries--;
	}

	/* Set new empty list header */
	dev->empty_list = last->next;
	/* Add new partial recv list to the head of reception list */
	last->next = dev->recv_list;
	/* Adjust rececption list pointer */
	dev->recv_list = first;

	rtcan_raw_print_filter(dev);
	rtcan_dev_dereference(dev);
    }

    return (flistlen) ? flistlen : 1;
}


void rtcan_raw_remove_filter(struct rtcan_socket *sock)
{
    int i, j, begin, end;
    struct rtcan_recv *first, *next, *last;
    int ifindex = atomic_read(&sock->ifindex);
    struct rtcan_device *dev;

    if (!rtcan_sock_has_filter(sock)) /* nothing to do */
	return;

    if (ifindex) {
	/* Socket was bound to one interface only. */
	begin = ifindex;
	end   = ifindex;
    } else {
	/* Socket was bound to all interfaces */
	begin = 1;
	end = RTCAN_MAX_DEVICES;
    }

    for (i = begin; i <= end; i++) {

	if ((dev = rtcan_dev_get_by_index(i)) == NULL)
	    continue;

	/* Search for first list entry pointing to this socket */
	first = NULL;
	next = dev->recv_list;
	while (next->sock != sock) {
	    first = next;
	    next = first->next;
	}

	/* Now go to the end of the old filter list */
	last = next;
	for (j = 1; j < sock->flistlen; j++)
	    last = last->next;

	/* Detach found first list entry from reception list */
	if (first)
	    first->next = last->next;
	else
	    dev->recv_list = last->next;
	/* Add partial list to the head of empty list */
	last->next = dev->empty_list;
	/* Adjust empty list pointer */
	dev->empty_list = next;

	/* Increase free entries counter by length of old filter list */
	dev->free_entries += sock->flistlen;

	rtcan_raw_print_filter(dev);
	rtcan_dev_dereference(dev);
    }
}