[linux.git] / net / caif / cfrfml.c

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

#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 kref *kref)
{
	struct cfsrvl *srvl = container_of(kref, struct cfsrvl, ref);
	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) {
		pr_warning("CAIF: %s(): Out of memory\n", __func__);
		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("CAIF: %s(): "
				"Connection error %d triggered on RFM link\n",
				__func__, 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);
	return err;
}


static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
{
	caif_assert(cfpkt_getlen(pkt) >= rfml->fragment_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))
		return err;

	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)
			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("CAIF: %s(): "
				"Connection error %d triggered on RFM link\n",
				__func__, 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 && frontpkt != pkt) {

			cfpkt_destroy(frontpkt);
			/*
			 * Socket layer will free the original packet,
			 * but this packet may already be sent and
			 * freed. So we have to return 0 in this case
			 * to avoid socket layer to re-free this packet.
			 * The return of shutdown indication will
			 * cause connection to be invalidated anyhow.
			 */
			err = 0;
		}
	}

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