linux-imx

/*

 * Copyright (C) ST-Ericsson AB 2010

 * Author:  Sjur Brendeland

 * 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;

​