[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / caif / cfrfml.c

/*
 * Copyright (C) ST-Ericsson AB 2010
 * Author:	Sjur Brendeland
 * License terms: GNU General Public License (GPL) version 2
 */

#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__

#include <linux/stddef.h>
#include <linux/spinlock.h>
#include <linux/slab.h>
#include <asm/unaligned.h>
#include <net/caif/caif_layer.h>
#include <net/caif/cfsrvl.h>
#include <net/caif/cfpkt.h>

#define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
#define RFM_SEGMENTATION_BIT 0x01
#define RFM_HEAD_SIZE 7

static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);

struct cfrfml {
	struct cfsrvl serv;
	struct cfpkt *incomplete_frm;
	int fragment_size;
	u8  seghead[6];
	u16 pdu_size;
	/* Protects serialized processing of packets */
	spinlock_t sync;
};

static void cfrfml_release(struct cflayer *layer)
{
	struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
	struct cfrfml *rfml = container_obj(&srvl->layer);

	if (rfml->incomplete_frm)
		cfpkt_destroy(rfml->incomplete_frm);

	kfree(srvl);
}

struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
			      int mtu_size)
{
	int tmp;
	struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);

	if (!this)
		return NULL;

	cfsrvl_init(&this->serv, channel_id, dev_info, false);
	this->serv.release = cfrfml_release;
	this->serv.layer.receive = cfrfml_receive;
	this->serv.layer.transmit = cfrfml_transmit;

	/* Round down to closest multiple of 16 */
	tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
	tmp *= 16;

	this->fragment_size = tmp;
	spin_lock_init(&this->sync);
	snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
		"rfm%d", channel_id);

	return &this->serv.layer;
}

static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
				struct cfpkt *pkt, int *err)
{
	struct cfpkt *tmppkt;
	*err = -EPROTO;
	/* n-th but not last segment */

	if (cfpkt_extr_head(pkt, seghead, 6) < 0)
		return NULL;

	/* Verify correct header */
	if (memcmp(seghead, rfml->seghead, 6) != 0)
		return NULL;

	tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
			rfml->pdu_size + RFM_HEAD_SIZE);

	/* If cfpkt_append failes input pkts are not freed */
	*err = -ENOMEM;
	if (tmppkt == NULL)
		return NULL;

	*err = 0;
	return tmppkt;
}

static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
{
	u8 tmp;
	bool segmented;
	int err;
	u8 seghead[6];
	struct cfrfml *rfml;
	struct cfpkt *tmppkt = NULL;

	caif_assert(layr->up != NULL);
	caif_assert(layr->receive != NULL);
	rfml = container_obj(layr);
	spin_lock(&rfml->sync);

	err = -EPROTO;
	if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
		goto out;
	segmented = tmp & RFM_SEGMENTATION_BIT;

	if (segmented) {
		if (rfml->incomplete_frm == NULL) {
			/* Initial Segment */
			if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0)
				goto out;

			rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);

			if (cfpkt_erroneous(pkt))
				goto out;
			rfml->incomplete_frm = pkt;
			pkt = NULL;
		} else {

			tmppkt = rfm_append(rfml, seghead, pkt, &err);
			if (tmppkt == NULL)
				goto out;

			if (cfpkt_erroneous(tmppkt))
				goto out;

			rfml->incomplete_frm = tmppkt;


			if (cfpkt_erroneous(tmppkt))
				goto out;
		}
		err = 0;
		goto out;
	}

	if (rfml->incomplete_frm) {

		/* Last Segment */
		tmppkt = rfm_append(rfml, seghead, pkt, &err);
		if (tmppkt == NULL)
			goto out;

		if (cfpkt_erroneous(tmppkt))
			goto out;

		rfml->incomplete_frm = NULL;
		pkt = tmppkt;
		tmppkt = NULL;

		/* Verify that length is correct */
		err = EPROTO;
		if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
			goto out;
	}

	err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);

out:

	if (err != 0) {
		if (tmppkt)
			cfpkt_destroy(tmppkt);
		if (pkt)
			cfpkt_destroy(pkt);
		if (rfml->incomplete_frm)
			cfpkt_destroy(rfml->incomplete_frm);
		rfml->incomplete_frm = NULL;

		pr_info("Connection error %d triggered on RFM link\n", err);

		/* Trigger connection error upon failure.*/
		layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
					rfml->serv.dev_info.id);
	}
	spin_unlock(&rfml->sync);

	if (unlikely(err == -EAGAIN))
		/* It is not possible to recover after drop of a fragment */
		err = -EIO;

	return err;
}


static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
{
	caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);

	/* Add info for MUX-layer to route the packet out. */
	cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;

	/*
	 * To optimize alignment, we add up the size of CAIF header before
	 * payload.
	 */
	cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
	cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;

	return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
}

static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
{
	int err;
	u8 seg;
	u8 head[6];
	struct cfpkt *rearpkt = NULL;
	struct cfpkt *frontpkt = pkt;
	struct cfrfml *rfml = container_obj(layr);

	caif_assert(layr->dn != NULL);
	caif_assert(layr->dn->transmit != NULL);

	if (!cfsrvl_ready(&rfml->serv, &err))
		goto out;

	err = -EPROTO;
	if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
		goto out;

	err = 0;
	if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
		err = cfpkt_peek_head(pkt, head, 6);

	if (err < 0)
		goto out;

	while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {

		seg = 1;
		err = -EPROTO;

		if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
			goto out;
		/*
		 * On OOM error cfpkt_split returns NULL.
		 *
		 * NOTE: Segmented pdu is not correctly aligned.
		 * This has negative performance impact.
		 */

		rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
		if (rearpkt == NULL)
			goto out;

		err = cfrfml_transmit_segment(rfml, frontpkt);

		if (err != 0) {
			frontpkt = NULL;
			goto out;
		}

		frontpkt = rearpkt;
		rearpkt = NULL;

		err = -ENOMEM;
		if (frontpkt == NULL)
			goto out;
		err = -EPROTO;
		if (cfpkt_add_head(frontpkt, head, 6) < 0)
			goto out;

	}

	seg = 0;
	err = -EPROTO;

	if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
		goto out;

	err = cfrfml_transmit_segment(rfml, frontpkt);

	frontpkt = NULL;
out:

	if (err != 0) {
		pr_info("Connection error %d triggered on RFM link\n", err);
		/* Trigger connection error upon failure.*/

		layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
					rfml->serv.dev_info.id);

		if (rearpkt)
			cfpkt_destroy(rearpkt);

		if (frontpkt)
			cfpkt_destroy(frontpkt);
	}

	return err;
}
Commit	Line	Data
b482cd20 SB	1	/*
b482cd20 SB	2	* Copyright (C) ST-Ericsson AB 2010
26ee65e6	3	* Author: Sjur Brendeland
b482cd20 SB	4	* License terms: GNU General Public License (GPL) version 2
	5	*/
	6
b31fa5ba JP	7	#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
b31fa5ba JP	8
b482cd20 SB	9	#include <linux/stddef.h>
	10	#include <linux/spinlock.h>
	11	#include <linux/slab.h>
7e368739	12	#include <asm/unaligned.h>
b482cd20 SB	13	#include <net/caif/caif_layer.h>
	14	#include <net/caif/cfsrvl.h>
	15	#include <net/caif/cfpkt.h>
	16
a7da1f55	17	#define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
b482cd20	18	#define RFM_SEGMENTATION_BIT 0x01
a7da1f55	19	#define RFM_HEAD_SIZE 7
b482cd20 SB	20
	21	static int cfrfml_receive(struct cflayer layr, struct cfpkt pkt);
	22	static int cfrfml_transmit(struct cflayer layr, struct cfpkt pkt);
b482cd20	23
a7da1f55 SB	24	struct cfrfml {
	25	struct cfsrvl serv;
	26	struct cfpkt *incomplete_frm;
	27	int fragment_size;
	28	u8 seghead[6];
	29	u16 pdu_size;
	30	/* Protects serialized processing of packets */
	31	spinlock_t sync;
	32	};
	33
43e36921	34	static void cfrfml_release(struct cflayer *layer)
a7da1f55	35	{
43e36921	36	struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
a7da1f55 SB	37	struct cfrfml *rfml = container_obj(&srvl->layer);
	38
	39	if (rfml->incomplete_frm)
	40	cfpkt_destroy(rfml->incomplete_frm);
	41
	42	kfree(srvl);
	43	}
	44
	45	struct cflayer cfrfml_create(u8 channel_id, struct dev_info dev_info,
3bffc475	46	int mtu_size)
b482cd20	47	{
a7da1f55	48	int tmp;
7ac2ed0c	49	struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
b1c74247	50
7ac2ed0c	51	if (!this)
b482cd20	52	return NULL;
b1c74247	53
a7da1f55 SB	54	cfsrvl_init(&this->serv, channel_id, dev_info, false);
	55	this->serv.release = cfrfml_release;
	56	this->serv.layer.receive = cfrfml_receive;
	57	this->serv.layer.transmit = cfrfml_transmit;
	58
	59	/* Round down to closest multiple of 16 */
	60	tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
	61	tmp *= 16;
	62
	63	this->fragment_size = tmp;
	64	spin_lock_init(&this->sync);
	65	snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
	66	"rfm%d", channel_id);
	67
	68	return &this->serv.layer;
	69	}
	70
	71	static struct cfpkt rfm_append(struct cfrfml rfml, char *seghead,
3bffc475	72	struct cfpkt pkt, int err)
a7da1f55 SB	73	{
	74	struct cfpkt *tmppkt;
	75	*err = -EPROTO;
	76	/* n-th but not last segment */
	77
	78	if (cfpkt_extr_head(pkt, seghead, 6) < 0)
	79	return NULL;
	80
	81	/* Verify correct header */
	82	if (memcmp(seghead, rfml->seghead, 6) != 0)
	83	return NULL;
	84
	85	tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
	86	rfml->pdu_size + RFM_HEAD_SIZE);
	87
	88	/* If cfpkt_append failes input pkts are not freed */
	89	*err = -ENOMEM;
	90	if (tmppkt == NULL)
	91	return NULL;
b1c74247	92
a7da1f55 SB	93	*err = 0;
a7da1f55 SB	94	return tmppkt;
b482cd20 SB	95	}
b482cd20 SB	96
b482cd20 SB	97	static int cfrfml_receive(struct cflayer layr, struct cfpkt pkt)
	98	{
	99	u8 tmp;
	100	bool segmented;
a7da1f55 SB	101	int err;
	102	u8 seghead[6];
	103	struct cfrfml *rfml;
	104	struct cfpkt *tmppkt = NULL;
	105
b482cd20 SB	106	caif_assert(layr->up != NULL);
b482cd20 SB	107	caif_assert(layr->receive != NULL);
a7da1f55 SB	108	rfml = container_obj(layr);
	109	spin_lock(&rfml->sync);
	110
	111	err = -EPROTO;
	112	if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
	113	goto out;
	114	segmented = tmp & RFM_SEGMENTATION_BIT;
	115
	116	if (segmented) {
	117	if (rfml->incomplete_frm == NULL) {
	118	/* Initial Segment */
	119	if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0)
	120	goto out;
	121
	122	rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
	123
	124	if (cfpkt_erroneous(pkt))
	125	goto out;
	126	rfml->incomplete_frm = pkt;
	127	pkt = NULL;
	128	} else {
	129
	130	tmppkt = rfm_append(rfml, seghead, pkt, &err);
	131	if (tmppkt == NULL)
	132	goto out;
	133
	134	if (cfpkt_erroneous(tmppkt))
	135	goto out;
	136
	137	rfml->incomplete_frm = tmppkt;
	138
	139
	140	if (cfpkt_erroneous(tmppkt))
	141	goto out;
	142	}
	143	err = 0;
	144	goto out;
	145	}
	146
	147	if (rfml->incomplete_frm) {
	148
	149	/* Last Segment */
	150	tmppkt = rfm_append(rfml, seghead, pkt, &err);
	151	if (tmppkt == NULL)
	152	goto out;
	153
	154	if (cfpkt_erroneous(tmppkt))
	155	goto out;
	156
	157	rfml->incomplete_frm = NULL;
	158	pkt = tmppkt;
	159	tmppkt = NULL;
	160
	161	/* Verify that length is correct */
	162	err = EPROTO;
	163	if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
	164	goto out;
	165	}
	166
	167	err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
	168
	169	out:
	170
	171	if (err != 0) {
172	if (tmppkt)
173	cfpkt_destroy(tmppkt);
174	if (pkt)
175	cfpkt_destroy(pkt);
176	if (rfml->incomplete_frm)
177	cfpkt_destroy(rfml->incomplete_frm);
178	rfml->incomplete_frm = NULL;
179
b31fa5ba	180	pr_info("Connection error %d triggered on RFM link\n", err);
a7da1f55 SB	181
	182	/* Trigger connection error upon failure.*/
	183	layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
	184	rfml->serv.dev_info.id);
	185	}
	186	spin_unlock(&rfml->sync);
374458b3 DT	187
	188	if (unlikely(err == -EAGAIN))
	189	/* It is not possible to recover after drop of a fragment */
	190	err = -EIO;
	191
a7da1f55 SB	192	return err;
	193	}
	194
	195
	196	static int cfrfml_transmit_segment(struct cfrfml rfml, struct cfpkt pkt)
	197	{
005b0b07	198	caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);
a7da1f55 SB	199
	200	/* Add info for MUX-layer to route the packet out. */
	201	cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
b482cd20 SB	202
b482cd20 SB	203	/*
a7da1f55 SB	204	* To optimize alignment, we add up the size of CAIF header before
a7da1f55 SB	205	* payload.
b482cd20	206	*/
a7da1f55 SB	207	cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
a7da1f55 SB	208	cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
b482cd20	209
a7da1f55	210	return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
b482cd20 SB	211	}
	212
	213	static int cfrfml_transmit(struct cflayer layr, struct cfpkt pkt)
	214	{
a7da1f55 SB	215	int err;
	216	u8 seg;
	217	u8 head[6];
	218	struct cfpkt *rearpkt = NULL;
	219	struct cfpkt *frontpkt = pkt;
	220	struct cfrfml *rfml = container_obj(layr);
b482cd20 SB	221
	222	caif_assert(layr->dn != NULL);
	223	caif_assert(layr->dn->transmit != NULL);
	224
a7da1f55	225	if (!cfsrvl_ready(&rfml->serv, &err))
374458b3	226	goto out;
a7da1f55 SB	227
	228	err = -EPROTO;
	229	if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
	230	goto out;
	231
	232	err = 0;
	233	if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
	234	err = cfpkt_peek_head(pkt, head, 6);
	235
	236	if (err < 0)
	237	goto out;
	238
	239	while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
	240
	241	seg = 1;
	242	err = -EPROTO;
	243
	244	if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
	245	goto out;
	246	/*
	247	* On OOM error cfpkt_split returns NULL.
	248	*
	249	* NOTE: Segmented pdu is not correctly aligned.
	250	* This has negative performance impact.
	251	*/
	252
	253	rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
	254	if (rearpkt == NULL)
	255	goto out;
	256
	257	err = cfrfml_transmit_segment(rfml, frontpkt);
	258
374458b3 DT	259	if (err != 0) {
374458b3 DT	260	frontpkt = NULL;
a7da1f55	261	goto out;
374458b3 DT	262	}
374458b3 DT	263
a7da1f55 SB	264	frontpkt = rearpkt;
	265	rearpkt = NULL;
	266
	267	err = -ENOMEM;
	268	if (frontpkt == NULL)
	269	goto out;
	270	err = -EPROTO;
	271	if (cfpkt_add_head(frontpkt, head, 6) < 0)
	272	goto out;
b482cd20	273
b482cd20	274	}
a7da1f55 SB	275
	276	seg = 0;
	277	err = -EPROTO;
	278
	279	if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
	280	goto out;
	281
	282	err = cfrfml_transmit_segment(rfml, frontpkt);
	283
	284	frontpkt = NULL;
	285	out:
	286
	287	if (err != 0) {
b31fa5ba	288	pr_info("Connection error %d triggered on RFM link\n", err);
a7da1f55 SB	289	/* Trigger connection error upon failure.*/
	290
	291	layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
	292	rfml->serv.dev_info.id);
	293
	294	if (rearpkt)
	295	cfpkt_destroy(rearpkt);
	296
374458b3	297	if (frontpkt)
a7da1f55	298	cfpkt_destroy(frontpkt);
b482cd20 SB	299	}
b482cd20 SB	300
a7da1f55	301	return err;
b482cd20	302	}