cfrfml.c

Go to the documentation of this file.

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

#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__

#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 cflayer *layer)
{
    struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer);
    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)
        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("Connection error %d triggered on RFM link\n", 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);

    if (unlikely(err == -EAGAIN))
        /* It is not possible to recover after drop of a fragment */
        err = -EIO;

    return err;
}


static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
{
    caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_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))
        goto out;

    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) {
            frontpkt = NULL;
            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("Connection error %d triggered on RFM link\n", 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)
            cfpkt_destroy(frontpkt);
    }

    return err;
}