Orange Pi5 kernel

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