linux/net/caif/cfrfml.c
<<
>>
Prefs
   1/*
   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
   5 */
   6
   7#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
   8
   9#include <linux/stddef.h>
  10#include <linux/spinlock.h>
  11#include <linux/slab.h>
  12#include <asm/unaligned.h>
  13#include <net/caif/caif_layer.h>
  14#include <net/caif/cfsrvl.h>
  15#include <net/caif/cfpkt.h>
  16
  17#define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
  18#define RFM_SEGMENTATION_BIT 0x01
  19#define RFM_HEAD_SIZE 7
  20
  21static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
  22static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
  23
  24struct 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
  34static void cfrfml_release(struct cflayer *layer)
  35{
  36        struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
  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
  45struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
  46                                        int mtu_size)
  47{
  48        int tmp;
  49        struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
  50
  51        if (!this)
  52                return NULL;
  53
  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
  71static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
  72                        struct cfpkt *pkt, int *err)
  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;
  92
  93        *err = 0;
  94        return tmppkt;
  95}
  96
  97static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
  98{
  99        u8 tmp;
 100        bool segmented;
 101        int err;
 102        u8 seghead[6];
 103        struct cfrfml *rfml;
 104        struct cfpkt *tmppkt = NULL;
 105
 106        caif_assert(layr->up != NULL);
 107        caif_assert(layr->receive != NULL);
 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
 169out:
 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
 180                pr_info("Connection error %d triggered on RFM link\n", err);
 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);
 187
 188        if (unlikely(err == -EAGAIN))
 189                /* It is not possible to recover after drop of a fragment */
 190                err = -EIO;
 191
 192        return err;
 193}
 194
 195
 196static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
 197{
 198        caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE);
 199
 200        /* Add info for MUX-layer to route the packet out. */
 201        cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
 202
 203        /*
 204         * To optimize alignment, we add up the size of CAIF header before
 205         * payload.
 206         */
 207        cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
 208        cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
 209
 210        return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
 211}
 212
 213static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
 214{
 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);
 221
 222        caif_assert(layr->dn != NULL);
 223        caif_assert(layr->dn->transmit != NULL);
 224
 225        if (!cfsrvl_ready(&rfml->serv, &err))
 226                goto out;
 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
 259                if (err != 0) {
 260                        frontpkt = NULL;
 261                        goto out;
 262                }
 263
 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;
 273
 274        }
 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;
 285out:
 286
 287        if (err != 0) {
 288                pr_info("Connection error %d triggered on RFM link\n", err);
 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
 297                if (frontpkt)
 298                        cfpkt_destroy(frontpkt);
 299        }
 300
 301        return err;
 302}
 303