// SPDX-License-Identifier: (GPL-2.0 WITH Linux-syscall-note) AND MIT
/*
 * af_stp.c - Linux kernel list API
 *
 * Authors: Octavian Guzu <octavian.guzu@gmail.com>
 *		Liza-Elena Babu <lizza.babu@gmail.com>
 */

#include "stp.h"
#include "af_stp.h"

DEFINE_RWLOCK(lock);
DEFINE_HASHTABLE(stp_hash, HT_SIZE);
struct proc_dir_entry *proc_list_read;
struct stp_stats global_stats;

static __u8 get_csum(char *start, char *end)
{
	__u8 res = 0;

	while (start != end) {
		res ^= *start;
		start++;
	}

	return res;
}

static int stp_release(struct socket *sock)
{
	struct stp_sock *s_stp;
	struct sock *sk;

	sk = sock->sk;
	s_stp = (struct stp_sock *)sk;

	if (!sk)
		return 0;

	if (s_stp->hash_node) {
		write_lock(&lock);
		hash_del(&s_stp->hash_node->next);
		kfree(s_stp->hash_node);
		write_unlock(&lock);
	}

	sock_orphan(sk);
	sock->sk = NULL;

	sock_put(sk);

	return 0;
}

static int stp_bind(struct socket *sock, struct sockaddr *uaddr, int addr_len)
{
	struct sockaddr_stp *sstp = (struct sockaddr_stp *) uaddr;
	struct stp_sock *stp_sock;
	struct hash_elem *elem = NULL;
	struct hlist_node *tmp = NULL;
	struct hash_elem *he = NULL;
	__be16 port;
	int err;

	err = -EINVAL;
	if (addr_len < sizeof(struct sockaddr_stp))
		goto out_err;

	err = -EINVAL;
	if (sstp->sas_family != AF_STP)
		goto out_err;

	err = -EINVAL;
	port = ntohs(sstp->sas_port);
	if (port < 1 || port > 65535)
		goto out_err;

	stp_sock = (struct stp_sock *) sock->sk;
	stp_sock->port_src = sstp->sas_port;
	stp_sock->if_idx = sstp->sas_ifindex;
	memcpy(stp_sock->mac_addr_src, sstp->sas_addr, MAC_ADDR_SIZE);

	read_lock(&lock);
	hash_for_each_possible_safe(stp_hash, he, tmp, next,
			stp_sock->port_src) {
		err = -EINVAL;
		read_unlock(&lock);
		goto out_err;
	}
	read_unlock(&lock);

	err = -ENOMEM;
	elem = kmalloc(sizeof(*elem), GFP_KERNEL);
	if (!elem)
		goto out_err;

	elem->hd.if_idx = stp_sock->if_idx;
	elem->hd.port = stp_sock->port_src;
	elem->hd.socket = stp_sock;
	stp_sock->hash_node = elem;

	write_lock(&lock);
	hash_add(stp_hash, &elem->next, stp_sock->port_src);
	write_unlock(&lock);

	return 0;

out_err:
	return err;

}

static int stp_connect(struct socket *sock, struct sockaddr *vaddr,
		int sockaddr_len, int flags)
{
	struct stp_sock *sstp;
	struct sockaddr_stp *addr;
	int sockaddr_stp_size;

	sstp = (struct stp_sock *)sock->sk;
	addr = (struct sockaddr_stp *)vaddr;
	sockaddr_stp_size = sizeof(struct sockaddr_stp);

	if (sockaddr_len != sockaddr_stp_size || addr->sas_family != AF_STP)
		return -EINVAL;

	memcpy(sstp->mac_addr_dst, addr->sas_addr, MAC_ADDR_SIZE);
	sstp->port_dst = addr->sas_port;

	return 0;
}

static int stp_sendmsg(struct socket *sock, struct msghdr *msg, size_t lenght)
{
	struct sockaddr_stp *addr_s;
	struct net_device *dev;
	struct stp_sock *stp_s;
	struct stp_hdr *hdr_s;
	struct sk_buff *skb;
	int offset_e, hdr_len, err, flags;
	__u8 *mac;
	__be16 port;

	err = -EINVAL;
	stp_s = (struct stp_sock *) sock->sk;
	dev = dev_get_by_index(sock_net(sock->sk), stp_s->if_idx);
	if (!dev)
		goto out;

	hdr_len = sizeof(struct stp_hdr) + lenght;
	flags = msg->msg_flags & MSG_DONTWAIT;
	skb = sock_alloc_send_pskb(sock->sk, LL_RESERVED_SPACE(dev),
			hdr_len, flags, &err, 0);
	if (!skb)
		goto out;

	skb_reserve(skb, LL_RESERVED_SPACE(dev));

	addr_s = (struct sockaddr_stp *) msg->msg_name;
	port = stp_s->port_dst ? stp_s->port_dst : addr_s->sas_port;
	mac = stp_s->port_dst ? stp_s->mac_addr_dst : addr_s->sas_addr;

	err = dev_hard_header(skb, dev, ETH_P_STP, mac,
			stp_s->mac_addr_src, hdr_len);
	offset_e = err;
	pr_alert("****** %d\n", offset_e);
	err = -EINVAL;
	hdr_s = (struct stp_hdr *) skb_put(skb, sizeof(struct stp_hdr));
	if (!hdr_s)
		goto out;

	hdr_s->len = htons(hdr_len);
	hdr_s->src = stp_s->port_src;
	hdr_s->dst = port;
	hdr_s->csum = 0;

	skb_put(skb, lenght);

	err = skb_copy_datagram_from_iter(skb,
			offset_e + sizeof(struct stp_hdr),
			&msg->msg_iter, lenght);
	if (err)
		goto out;

	hdr_s->csum = get_csum(skb->data + offset_e, skb->tail);

	skb->dev = dev;
	skb->protocol = htons(ETH_P_STP);
	skb->priority = sock->sk->sk_priority;

	err = dev_queue_xmit(skb);
	if (err)
		goto out;

	global_stats.txpkts++;

	dev_put(dev);
	err = lenght;

out:
	return err;
}

static int stp_recvmsg(struct socket *sock, struct msghdr *msg,
		size_t len, int flags)
{
	struct sockaddr_stp *addr_s;
	struct stp_sock *stp_s;
	struct stp_hdr *hdr_s;
	struct sk_buff *skb;
	struct ethhdr *eth;
	int err;

	skb = skb_recv_datagram(sock->sk, flags, flags & MSG_DONTWAIT, &err);
	err = -EINVAL;
	if (!skb)
		goto out;

	err = -EINVAL;
	if (get_csum(skb->data, skb->tail)) {
		global_stats.csumerr++;
		goto skb_release_datagram;
	}

	err = skb_copy_datagram_iter(skb, sizeof(struct stp_hdr),
			&msg->msg_iter, len);
	if (err)
		goto skb_release_datagram;

	addr_s = (struct sockaddr_stp *) msg->msg_name;
	stp_s = (struct stp_sock *) sock->sk;
	hdr_s = (struct stp_hdr *) skb->data;
	eth = eth_hdr(skb);
	if (eth && addr_s) {
		addr_s->sas_port = hdr_s->src;
		addr_s->sas_family = AF_STP;
		memcpy(addr_s->sas_addr, eth->h_source, MAC_ADDR_SIZE);
	}

	sock_recv_ts_and_drops(msg, sock->sk, skb);

	global_stats.rxpkts++;
	err = len;

skb_release_datagram:
	skb_free_datagram(sock->sk, skb);
out:
	return err;
}

static int stp_packet_recv(struct sk_buff *skb, struct net_device *dev,
		struct packet_type *p_type, struct net_device *orig_dev)
{
	struct stp_hdr *stp_header;
	struct hlist_node *tmp = NULL;
	struct hash_elem *he = NULL;
	struct stp_sock *stp_s = NULL;
	__be16 dst, src;
	int err;

	err = -EINVAL;
	if (skb->len < sizeof(struct stp_hdr)) {
		global_stats.hdrerr++;
		goto out;
	}

	stp_header = (struct stp_hdr *) skb->data;
	dst = stp_header->dst;
	src = stp_header->src;

	err = -EINVAL;
	if (!dst || !src) {
		global_stats.hdrerr++;
		goto out;
	}

	read_lock(&lock);
	hash_for_each_possible_safe(stp_hash, he, tmp, next, dst) {
		stp_s = he->hd.socket;
	}
	read_unlock(&lock);

	err = -EINVAL;
	if (stp_s == NULL) {
		global_stats.nosock++;
		goto out;
	}

	err = sock_queue_rcv_skb(&stp_s->sk, skb);
	if (err) {
		global_stats.nobuffs++;
		goto out;
	}

	return 0;
out:
	return err;
}

static int stp_packet_create(struct net *net, struct socket *sock,
		int protocol, int kern)
{

	struct sock *sk;
	int err;

	if (sock->type != SOCK_DGRAM)
		return -ESOCKTNOSUPPORT;

	err = -EINVAL;
	if (protocol)
		goto out;

	sock->state = SS_UNCONNECTED;

	err = -ENOBUFS;
	sk = sk_alloc(net, PF_STP, GFP_KERNEL, &stp_proto, kern);
	if (!sk)
		goto out;

	sock->ops = &stp_proto_ops;

	sock_init_data(sock, sk);

	sk->sk_family = PF_STP;

	return 0;
out:
	return err;
}

static int stp_proc_show(struct seq_file *m, void *v)
{
	seq_puts(m, "RxPkts HdrErr CsumErr NoSock NoBuffs TxPkts\n");

	seq_printf(m, "%d %d %d %d %d %d\n",
			global_stats.rxpkts,
			global_stats.hdrerr,
			global_stats.csumerr,
			global_stats.nosock,
			global_stats.nobuffs,
			global_stats.txpkts);

	return 0;
}

static int stp_read_open(struct inode *inode, struct  file *file)
{
	return single_open(file, stp_proc_show, NULL);
}

static const struct file_operations r_fops = {
	.owner		= THIS_MODULE,
	.open		= stp_read_open,
	.read		= seq_read,
	.release	= single_release,
};

static int stp_init(void)
{
	int err;

	err = -ENOMEM;
	proc_list_read = proc_create(STP_PROC_NET_FILENAME, 0000,
			init_net.proc_net, &r_fops);
	if (!proc_list_read)
		goto out;

	err = proto_register(&stp_proto, 0);
	if (err)
		goto proto_reg_fail;

	err = sock_register(&stp_packet_family_ops);
	if (err)
		goto sock_reg_fail;

	dev_add_pack(&stp_packet_type);

	return 0;

sock_reg_fail:
	proto_unregister(&stp_proto);
proto_reg_fail:
	proc_remove(proc_list_read);
out:
	return err;
}

static void stp_exit(void)
{
	struct hlist_node *tmp = NULL;
	struct hash_elem *he = NULL;
	int count = 0;

	dev_remove_pack(&stp_packet_type);
	proc_remove(proc_list_read);
	proto_unregister(&stp_proto);
	sock_unregister(PF_STP);

	write_lock(&lock);
	hash_for_each_safe(stp_hash, count, tmp, he, next) {
		hash_del(tmp);
		kfree(he);
	}
	write_unlock(&lock);
}

module_init(stp_init);
module_exit(stp_exit);

MODULE_DESCRIPTION("Linux kernel networking protocol");
MODULE_AUTHOR("Octavian Guzu & Liza Babu");
MODULE_LICENSE("GPL v2");