net/caif/cfrfml.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Copyright (C) ST-Ericsson AB 2010
0004  * Author:  Sjur Brendeland
0005  */
0006
0007 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
0008
0009 #include <linux/stddef.h>
0010 #include <linux/spinlock.h>
0011 #include <linux/slab.h>
0012 #include <asm/unaligned.h>
0013 #include <net/caif/caif_layer.h>
0014 #include <net/caif/cfsrvl.h>
0015 #include <net/caif/cfpkt.h>
0016
0017 #define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
0018 #define RFM_SEGMENTATION_BIT 0x01
0019 #define RFM_HEAD_SIZE 7
0020
0021 static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
0022 static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
0023
0024 struct cfrfml {
0025     struct cfsrvl serv;
0026     struct cfpkt *incomplete_frm;
0027     int fragment_size;
0028     u8  seghead[6];
0029     u16 pdu_size;
0030     /* Protects serialized processing of packets */
0031     spinlock_t sync;
0032 };
0033
0034 static void cfrfml_release(struct cflayer *layer)
0035 {
0036     struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
0037     struct cfrfml *rfml = container_obj(&srvl->layer);
0038
0039     if (rfml->incomplete_frm)
0040         cfpkt_destroy(rfml->incomplete_frm);
0041
0042     kfree(srvl);
0043 }
0044
0045 struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
0046                   int mtu_size)
0047 {
0048     int tmp;
0049     struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
0050
0051     if (!this)
0052         return NULL;
0053
0054     cfsrvl_init(&this->serv, channel_id, dev_info, false);
0055     this->serv.release = cfrfml_release;
0056     this->serv.layer.receive = cfrfml_receive;
0057     this->serv.layer.transmit = cfrfml_transmit;
0058
0059     /* Round down to closest multiple of 16 */
0060     tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
0061     tmp *= 16;
0062
0063     this->fragment_size = tmp;
0064     spin_lock_init(&this->sync);
0065     snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
0066         "rfm%d", channel_id);
0067
0068     return &this->serv.layer;
0069 }
0070
0071 static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
0072                 struct cfpkt *pkt, int *err)
0073 {
0074     struct cfpkt *tmppkt;
0075     *err = -EPROTO;
0076     /* n-th but not last segment */
0077
0078     if (cfpkt_extr_head(pkt, seghead, 6) < 0)
0079         return NULL;
0080
0081     /* Verify correct header */
0082     if (memcmp(seghead, rfml->seghead, 6) != 0)
0083         return NULL;
0084
0085     tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
0086             rfml->pdu_size + RFM_HEAD_SIZE);
0087
0088     /* If cfpkt_append failes input pkts are not freed */
0089     *err = -ENOMEM;
0090     if (tmppkt == NULL)
0091         return NULL;
0092
0093     *err = 0;
0094     return tmppkt;
0095 }
0096
0097 static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
0098 {
0099     u8 tmp;
0100     bool segmented;
0101     int err;
0102     u8 seghead[6];
0103     struct cfrfml *rfml;
0104     struct cfpkt *tmppkt = NULL;
0105
0106     caif_assert(layr->up != NULL);
0107     caif_assert(layr->receive != NULL);
0108     rfml = container_obj(layr);
0109     spin_lock(&rfml->sync);
0110
0111     err = -EPROTO;
0112     if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
0113         goto out;
0114     segmented = tmp & RFM_SEGMENTATION_BIT;
0115
0116     if (segmented) {
0117         if (rfml->incomplete_frm == NULL) {
0118             /* Initial Segment */
0119             if (cfpkt_peek_head(pkt, rfml->seghead, 6) != 0)
0120                 goto out;
0121
0122             rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
0123
0124             if (cfpkt_erroneous(pkt))
0125                 goto out;
0126             rfml->incomplete_frm = pkt;
0127             pkt = NULL;
0128         } else {
0129
0130             tmppkt = rfm_append(rfml, seghead, pkt, &err);
0131             if (tmppkt == NULL)
0132                 goto out;
0133
0134             if (cfpkt_erroneous(tmppkt))
0135                 goto out;
0136
0137             rfml->incomplete_frm = tmppkt;
0138
0139
0140             if (cfpkt_erroneous(tmppkt))
0141                 goto out;
0142         }
0143         err = 0;
0144         goto out;
0145     }
0146
0147     if (rfml->incomplete_frm) {
0148
0149         /* Last Segment */
0150         tmppkt = rfm_append(rfml, seghead, pkt, &err);
0151         if (tmppkt == NULL)
0152             goto out;
0153
0154         if (cfpkt_erroneous(tmppkt))
0155             goto out;
0156
0157         rfml->incomplete_frm = NULL;
0158         pkt = tmppkt;
0159         tmppkt = NULL;
0160
0161         /* Verify that length is correct */
0162         err = -EPROTO;
0163         if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
0164             goto out;
0165     }
0166
0167     err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
0168
0169 out:
0170
0171     if (err != 0) {
0172         if (tmppkt)
0173             cfpkt_destroy(tmppkt);
0174         if (pkt)
0175             cfpkt_destroy(pkt);
0176         if (rfml->incomplete_frm)
0177             cfpkt_destroy(rfml->incomplete_frm);
0178         rfml->incomplete_frm = NULL;
0179
0180         pr_info("Connection error %d triggered on RFM link\n", err);
0181
0182         /* Trigger connection error upon failure.*/
0183         layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
0184                     rfml->serv.dev_info.id);
0185     }
0186     spin_unlock(&rfml->sync);
0187
0188     if (unlikely(err == -EAGAIN))
0189         /* It is not possible to recover after drop of a fragment */
0190         err = -EIO;
0191
0192     return err;
0193 }
0194
0195
0196 static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
0197 {
0198     caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);
0199
0200     /* Add info for MUX-layer to route the packet out. */
0201     cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
0202
0203     /*
0204      * To optimize alignment, we add up the size of CAIF header before
0205      * payload.
0206      */
0207     cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
0208     cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
0209
0210     return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
0211 }
0212
0213 static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
0214 {
0215     int err;
0216     u8 seg;
0217     u8 head[6];
0218     struct cfpkt *rearpkt = NULL;
0219     struct cfpkt *frontpkt = pkt;
0220     struct cfrfml *rfml = container_obj(layr);
0221
0222     caif_assert(layr->dn != NULL);
0223     caif_assert(layr->dn->transmit != NULL);
0224
0225     if (!cfsrvl_ready(&rfml->serv, &err))
0226         goto out;
0227
0228     err = -EPROTO;
0229     if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
0230         goto out;
0231
0232     err = 0;
0233     if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
0234         err = cfpkt_peek_head(pkt, head, 6);
0235
0236     if (err != 0)
0237         goto out;
0238
0239     while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
0240
0241         seg = 1;
0242         err = -EPROTO;
0243
0244         if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
0245             goto out;
0246         /*
0247          * On OOM error cfpkt_split returns NULL.
0248          *
0249          * NOTE: Segmented pdu is not correctly aligned.
0250          * This has negative performance impact.
0251          */
0252
0253         rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
0254         if (rearpkt == NULL)
0255             goto out;
0256
0257         err = cfrfml_transmit_segment(rfml, frontpkt);
0258
0259         if (err != 0) {
0260             frontpkt = NULL;
0261             goto out;
0262         }
0263
0264         frontpkt = rearpkt;
0265         rearpkt = NULL;
0266
0267         err = -EPROTO;
0268         if (cfpkt_add_head(frontpkt, head, 6) < 0)
0269             goto out;
0270
0271     }
0272
0273     seg = 0;
0274     err = -EPROTO;
0275
0276     if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
0277         goto out;
0278
0279     err = cfrfml_transmit_segment(rfml, frontpkt);
0280
0281     frontpkt = NULL;
0282 out:
0283
0284     if (err != 0) {
0285         pr_info("Connection error %d triggered on RFM link\n", err);
0286         /* Trigger connection error upon failure.*/
0287
0288         layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
0289                     rfml->serv.dev_info.id);
0290
0291         if (rearpkt)
0292             cfpkt_destroy(rearpkt);
0293
0294         if (frontpkt)
0295             cfpkt_destroy(frontpkt);
0296     }
0297
0298     return err;
0299 }