pfe/c2000/control_l2tp.c

/*
 *  Copyright (c) 2013 Mindspeed Technologies, Inc.
 *
 *  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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 *
 */
#include "fpp_globals.h"
#include "modules.h"
#include "events.h"
#include "module_Rx.h"
#include "gemac.h"
#include "fpool.h"
#include "fpp.h"
#include "layer2.h"
#include "module_hidrv.h"
#include "module_timer.h"
#include "module_expt.h"
#include "module_ethernet.h"
#include "module_ipv4.h"
#include "module_ipv6.h"
#include "module_stat.h"
#include "system.h"
#include "fe.h"
#include "channels.h"
#include "module_l2tp.h"
#include "control_common.h"


struct slist_head CLASS_PE_LMEM_SH(l2tp_cache)[NUM_L2TP_ENTRIES];
l2tp_entry CLASS_PE_LMEM_SH(l2tp_table)[MAX_L2TP_ITF];


static pl2tp_entry l2tp_get_entry(void)
{
	int i = 0;
	pl2tp_entry pEntry;

	for (i = 0; i < MAX_L2TP_ITF; i++)
	{
		pEntry = &l2tp_table[i];
		/* Tunnel ID cannot be NULL, we use it as a free/busy flag */
		if (pEntry->local_tun_id == L2TP_IF_FREE)
			return pEntry;
	}
	return NULL;
}

static int l2tp_add(pl2tp_entry sw_entry, U32 hash)
{
	struct pfe_ctrl *ctrl = &pfe->ctrl;
	l2tp_entry hw_entry;

	/* Add software entry to local hash */
	slist_add(&l2tp_cache[hash], &sw_entry->list);
	
	/* Setup HW entry */

	memcpy(&hw_entry, sw_entry, sizeof(l2tp_entry));

	//sw_entry->hash = hash;
	hw_entry.sock_id = cpu_to_be16(sw_entry->sock_id);
	hw_entry.options = cpu_to_be16(sw_entry->options);
	
	hw_entry.itf.phys = (void *)cpu_to_be32(virt_to_class_pe_lmem(sw_entry->itf.phys));
	slist_set_next(&hw_entry.list, (void *)cpu_to_be32(virt_to_class_pe_lmem(slist_next(&sw_entry->list))));


	/* Update interface in PE cluster mem */
	class_pe_lmem_memcpy_to32(virt_to_class_pe_lmem(sw_entry), &hw_entry, sizeof(l2tp_entry));

	pe_sync_stop(ctrl, CLASS_MASK);

	/* Update PE cluster mem hash table  */
	class_bus_write(cpu_to_be32(virt_to_class_pe_lmem(&sw_entry->list)), virt_to_class_pe_lmem(&l2tp_cache[hash]), sizeof(struct slist_entry *));


	pe_start(ctrl, CLASS_MASK);


	return NO_ERR;
}

static void l2tp_remove(pl2tp_entry sw_entry, U32 hash)
{
	struct pfe_ctrl *ctrl = &pfe->ctrl;
	struct slist_entry *prev;

	/* Now remove the interface PE cluster mem */
	prev = slist_prev(&l2tp_cache[hash], &sw_entry->list);
	
	pe_sync_stop(ctrl, CLASS_MASK);

	class_bus_write(cpu_to_be32(virt_to_class_pe_lmem(slist_next(&sw_entry->list))),virt_to_class_pe_lmem(prev), sizeof(struct slist_entry *));

	pe_start(ctrl, CLASS_MASK);

	/* Remove from our local hash */
	slist_remove_after(prev);

	/* Flag entry as free */
	sw_entry->local_tun_id = L2TP_IF_FREE;

}

static pl2tp_entry l2tp_find_entry(U32 hash, U16 local_tun_id, U16 peer_tun_id, U16 local_ses_id, U16 peer_ses_id)
{
	pl2tp_entry pEntry;
	struct slist_entry *entry;

	slist_for_each(pEntry, entry, &l2tp_cache[hash], list) {
		if ((pEntry->local_tun_id == local_tun_id) && (pEntry->peer_tun_id == peer_tun_id)
		&& (pEntry->local_ses_id == local_ses_id) && (pEntry->peer_ses_id == peer_ses_id))
			return pEntry;
	}
	return NULL;
}

U16 M_l2tp_cmdproc(U16 cmd_code, U16 cmd_len, U16 *p)
{
	U16 acklen;
	U16 ackstatus;
	pl2tp_entry l2tp_entry;

	acklen = 2;
	ackstatus = CMD_OK;

	switch (cmd_code)
	{
	case CMD_L2TP_ITF_ADD: {
		U32 hash;
		pl2tp_itf_add_cmd pcmd = (pl2tp_itf_add_cmd)p;

		if(get_onif_by_name(pcmd->ifname)) {
			ackstatus = ERR_UNKNOWN_INTERFACE;	
			break;
		}
		
		hash = HASH_L2TP(pcmd->local_tun_id, pcmd->local_ses_id);
		if (l2tp_find_entry(hash, pcmd->local_tun_id, pcmd->peer_tun_id, pcmd->local_ses_id, pcmd->peer_ses_id)) {
			ackstatus = ERR_CREATION_FAILED;
			break;
		}

		l2tp_entry = l2tp_get_entry();
		if(!l2tp_entry)
		{
			ackstatus = ERR_CREATION_FAILED;
			break;
		}

		l2tp_entry->sock_id = pcmd->sock_id;
		l2tp_entry->local_tun_id = pcmd->local_tun_id;
		l2tp_entry->peer_tun_id = pcmd->peer_tun_id;
		l2tp_entry->local_ses_id = pcmd->local_ses_id;
		l2tp_entry->peer_ses_id = pcmd->peer_ses_id;
		l2tp_entry->options = pcmd->options;

#if 0 //DEBUG
		printk(KERN_INFO "sock_id(%d)\n", l2tp_entry->sock_id);
		printk(KERN_INFO "local_tun_id(%d)\n", l2tp_entry->local_tun_id);
		printk(KERN_INFO "peer_tun_id(%d)\n", l2tp_entry->peer_tun_id);
		printk(KERN_INFO "local_ses_id(%d)\n", l2tp_entry->local_ses_id);
		printk(KERN_INFO "peer_ses_id(%d)\n", l2tp_entry->peer_ses_id);
		printk(KERN_INFO "hash(%d)\n", hash);
#endif


		/* Now create a new interface in the Interface Manager */
		if (!add_onif(pcmd->ifname, &l2tp_entry->itf, NULL, IF_TYPE_L2TP)) {
			ackstatus = ERR_CREATION_FAILED;
			l2tp_entry->local_tun_id = L2TP_IF_FREE;
			break;
		}
		if((ackstatus = l2tp_add(l2tp_entry, hash)) != NO_ERR)
		{
			/* Remove onif already created*/
			remove_onif_by_index(l2tp_entry->itf.index);

			/* Free up the l2tp_entry for later use*/
			l2tp_entry->local_tun_id = L2TP_IF_FREE;
			break;
		}

		break;
	}
	case CMD_L2TP_ITF_DEL: {
		pl2tp_itf_del_cmd pcmd = (pl2tp_itf_del_cmd)p;
		POnifDesc pOnif;
		pl2tp_entry pEntry;
		U32 hash;

		if((pOnif = get_onif_by_name(pcmd->ifname)) == NULL) {
			ackstatus = ERR_UNKNOWN_INTERFACE;	
			break;
		}
		pEntry = container_of(pOnif->itf, typeof(struct _tl2tp_entry), itf);
		remove_onif_by_index(pEntry->itf.index);

		hash = HASH_L2TP(pEntry->local_tun_id,pEntry->local_ses_id);

		l2tp_remove(pEntry, hash);
		break;
	}
		ackstatus = ERR_UNKNOWN_COMMAND;
		break;
	}
	*p = ackstatus;
	return acklen;
}

int l2tp_init(void)
{
	int i;

	set_cmd_handler(EVENT_L2TP, M_l2tp_cmdproc);

	for (i = 0; i < NUM_L2TP_ENTRIES; i++)
		slist_head_init(&l2tp_cache[i]);

	return 0;
}

void l2tp_exit(void)
{
	return;
}