2 * Copyright (C) ST-Ericsson AB 2010
3 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com
4 * License terms: GNU General Public License (GPL) version 2
7 #include <linux/stddef.h>
8 #include <linux/spinlock.h>
9 #include <linux/slab.h>
10 #include <linux/unaligned/le_byteshift.h>
11 #include <net/caif/caif_layer.h>
12 #include <net/caif/cfsrvl.h>
13 #include <net/caif/cfpkt.h>
15 #define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
16 #define RFM_SEGMENTATION_BIT 0x01
17 #define RFM_HEAD_SIZE 7
19 static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
20 static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
24 struct cfpkt *incomplete_frm;
28 /* Protects serialized processing of packets */
32 static void cfrfml_release(struct kref *kref)
34 struct cfsrvl *srvl = container_of(kref, struct cfsrvl, ref);
35 struct cfrfml *rfml = container_obj(&srvl->layer);
37 if (rfml->incomplete_frm)
38 cfpkt_destroy(rfml->incomplete_frm);
43 struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
48 kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
51 pr_warning("CAIF: %s(): Out of memory\n", __func__);
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;
60 /* Round down to closest multiple of 16 */
61 tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
64 this->fragment_size = tmp;
65 spin_lock_init(&this->sync);
66 snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
69 return &this->serv.layer;
72 static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
73 struct cfpkt *pkt, int *err)
77 /* n-th but not last segment */
79 if (cfpkt_extr_head(pkt, seghead, 6) < 0)
82 /* Verify correct header */
83 if (memcmp(seghead, rfml->seghead, 6) != 0)
86 tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
87 rfml->pdu_size + RFM_HEAD_SIZE);
89 /* If cfpkt_append failes input pkts are not freed */
98 static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
105 struct cfpkt *tmppkt = NULL;
107 caif_assert(layr->up != NULL);
108 caif_assert(layr->receive != NULL);
109 rfml = container_obj(layr);
110 spin_lock(&rfml->sync);
113 if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
115 segmented = tmp & RFM_SEGMENTATION_BIT;
118 if (rfml->incomplete_frm == NULL) {
119 /* Initial Segment */
120 if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0)
123 rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
125 if (cfpkt_erroneous(pkt))
127 rfml->incomplete_frm = pkt;
131 tmppkt = rfm_append(rfml, seghead, pkt, &err);
135 if (cfpkt_erroneous(tmppkt))
138 rfml->incomplete_frm = tmppkt;
141 if (cfpkt_erroneous(tmppkt))
148 if (rfml->incomplete_frm) {
151 tmppkt = rfm_append(rfml, seghead, pkt, &err);
155 if (cfpkt_erroneous(tmppkt))
158 rfml->incomplete_frm = NULL;
162 /* Verify that length is correct */
164 if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
168 err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
174 cfpkt_destroy(tmppkt);
177 if (rfml->incomplete_frm)
178 cfpkt_destroy(rfml->incomplete_frm);
179 rfml->incomplete_frm = NULL;
181 pr_info("CAIF: %s(): "
182 "Connection error %d triggered on RFM link\n",
185 /* Trigger connection error upon failure.*/
186 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
187 rfml->serv.dev_info.id);
189 spin_unlock(&rfml->sync);
194 static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
196 caif_assert(cfpkt_getlen(pkt) >= rfml->fragment_size);
198 /* Add info for MUX-layer to route the packet out. */
199 cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
202 * To optimize alignment, we add up the size of CAIF header before
205 cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
206 cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
208 return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
211 static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
216 struct cfpkt *rearpkt = NULL;
217 struct cfpkt *frontpkt = pkt;
218 struct cfrfml *rfml = container_obj(layr);
220 caif_assert(layr->dn != NULL);
221 caif_assert(layr->dn->transmit != NULL);
223 if (!cfsrvl_ready(&rfml->serv, &err))
227 if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
231 if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
232 err = cfpkt_peek_head(pkt, head, 6);
237 while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
242 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
245 * On OOM error cfpkt_split returns NULL.
247 * NOTE: Segmented pdu is not correctly aligned.
248 * This has negative performance impact.
251 rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
255 err = cfrfml_transmit_segment(rfml, frontpkt);
263 if (frontpkt == NULL)
266 if (cfpkt_add_head(frontpkt, head, 6) < 0)
274 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
277 err = cfrfml_transmit_segment(rfml, frontpkt);
283 pr_info("CAIF: %s(): "
284 "Connection error %d triggered on RFM link\n",
286 /* Trigger connection error upon failure.*/
288 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
289 rfml->serv.dev_info.id);
292 cfpkt_destroy(rearpkt);
294 if (frontpkt && frontpkt != pkt) {
296 cfpkt_destroy(frontpkt);
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.