linux/net/caif/cffrml.c
<<
>>
Prefs
   1/*
   2 * CAIF Framing Layer.
   3 *
   4 * Copyright (C) ST-Ericsson AB 2010
   5 * Author:      Sjur Brendeland/sjur.brandeland@stericsson.com
   6 * License terms: GNU General Public License (GPL) version 2
   7 */
   8
   9#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
  10
  11#include <linux/stddef.h>
  12#include <linux/spinlock.h>
  13#include <linux/slab.h>
  14#include <linux/crc-ccitt.h>
  15#include <linux/netdevice.h>
  16#include <net/caif/caif_layer.h>
  17#include <net/caif/cfpkt.h>
  18#include <net/caif/cffrml.h>
  19
  20#define container_obj(layr) container_of(layr, struct cffrml, layer)
  21
  22struct cffrml {
  23        struct cflayer layer;
  24        bool dofcs;             /* !< FCS active */
  25        int __percpu            *pcpu_refcnt;
  26};
  27
  28static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt);
  29static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt);
  30static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
  31                                int phyid);
  32
  33static u32 cffrml_rcv_error;
  34static u32 cffrml_rcv_checsum_error;
  35struct cflayer *cffrml_create(u16 phyid, bool use_fcs)
  36{
  37        struct cffrml *this = kzalloc(sizeof(struct cffrml), GFP_ATOMIC);
  38        if (!this)
  39                return NULL;
  40        this->pcpu_refcnt = alloc_percpu(int);
  41        if (this->pcpu_refcnt == NULL) {
  42                kfree(this);
  43                return NULL;
  44        }
  45
  46        caif_assert(offsetof(struct cffrml, layer) == 0);
  47
  48        this->layer.receive = cffrml_receive;
  49        this->layer.transmit = cffrml_transmit;
  50        this->layer.ctrlcmd = cffrml_ctrlcmd;
  51        snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "frm%d", phyid);
  52        this->dofcs = use_fcs;
  53        this->layer.id = phyid;
  54        return (struct cflayer *) this;
  55}
  56
  57void cffrml_free(struct cflayer *layer)
  58{
  59        struct cffrml *this = container_obj(layer);
  60        free_percpu(this->pcpu_refcnt);
  61        kfree(layer);
  62}
  63
  64void cffrml_set_uplayer(struct cflayer *this, struct cflayer *up)
  65{
  66        this->up = up;
  67}
  68
  69void cffrml_set_dnlayer(struct cflayer *this, struct cflayer *dn)
  70{
  71        this->dn = dn;
  72}
  73
  74static u16 cffrml_checksum(u16 chks, void *buf, u16 len)
  75{
  76        /* FIXME: FCS should be moved to glue in order to use OS-Specific
  77         * solutions
  78         */
  79        return crc_ccitt(chks, buf, len);
  80}
  81
  82static int cffrml_receive(struct cflayer *layr, struct cfpkt *pkt)
  83{
  84        u16 tmp;
  85        u16 len;
  86        u16 hdrchks;
  87        u16 pktchks;
  88        struct cffrml *this;
  89        this = container_obj(layr);
  90
  91        cfpkt_extr_head(pkt, &tmp, 2);
  92        len = le16_to_cpu(tmp);
  93
  94        /* Subtract for FCS on length if FCS is not used. */
  95        if (!this->dofcs)
  96                len -= 2;
  97
  98        if (cfpkt_setlen(pkt, len) < 0) {
  99                ++cffrml_rcv_error;
 100                pr_err("Framing length error (%d)\n", len);
 101                cfpkt_destroy(pkt);
 102                return -EPROTO;
 103        }
 104        /*
 105         * Don't do extract if FCS is false, rather do setlen - then we don't
 106         * get a cache-miss.
 107         */
 108        if (this->dofcs) {
 109                cfpkt_extr_trail(pkt, &tmp, 2);
 110                hdrchks = le16_to_cpu(tmp);
 111                pktchks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff);
 112                if (pktchks != hdrchks) {
 113                        cfpkt_add_trail(pkt, &tmp, 2);
 114                        ++cffrml_rcv_error;
 115                        ++cffrml_rcv_checsum_error;
 116                        pr_info("Frame checksum error (0x%x != 0x%x)\n",
 117                                hdrchks, pktchks);
 118                        return -EILSEQ;
 119                }
 120        }
 121        if (cfpkt_erroneous(pkt)) {
 122                ++cffrml_rcv_error;
 123                pr_err("Packet is erroneous!\n");
 124                cfpkt_destroy(pkt);
 125                return -EPROTO;
 126        }
 127
 128        if (layr->up == NULL) {
 129                pr_err("Layr up is missing!\n");
 130                cfpkt_destroy(pkt);
 131                return -EINVAL;
 132        }
 133
 134        return layr->up->receive(layr->up, pkt);
 135}
 136
 137static int cffrml_transmit(struct cflayer *layr, struct cfpkt *pkt)
 138{
 139        u16 chks;
 140        u16 len;
 141        __le16 data;
 142
 143        struct cffrml *this = container_obj(layr);
 144        if (this->dofcs) {
 145                chks = cfpkt_iterate(pkt, cffrml_checksum, 0xffff);
 146                data = cpu_to_le16(chks);
 147                cfpkt_add_trail(pkt, &data, 2);
 148        } else {
 149                cfpkt_pad_trail(pkt, 2);
 150        }
 151        len = cfpkt_getlen(pkt);
 152        data = cpu_to_le16(len);
 153        cfpkt_add_head(pkt, &data, 2);
 154        cfpkt_info(pkt)->hdr_len += 2;
 155        if (cfpkt_erroneous(pkt)) {
 156                pr_err("Packet is erroneous!\n");
 157                cfpkt_destroy(pkt);
 158                return -EPROTO;
 159        }
 160
 161        if (layr->dn == NULL) {
 162                cfpkt_destroy(pkt);
 163                return -ENODEV;
 164
 165        }
 166        return layr->dn->transmit(layr->dn, pkt);
 167}
 168
 169static void cffrml_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl,
 170                                        int phyid)
 171{
 172        if (layr->up && layr->up->ctrlcmd)
 173                layr->up->ctrlcmd(layr->up, ctrl, layr->id);
 174}
 175
 176void cffrml_put(struct cflayer *layr)
 177{
 178        struct cffrml *this = container_obj(layr);
 179        if (layr != NULL && this->pcpu_refcnt != NULL)
 180                this_cpu_dec(*this->pcpu_refcnt);
 181}
 182
 183void cffrml_hold(struct cflayer *layr)
 184{
 185        struct cffrml *this = container_obj(layr);
 186        if (layr != NULL && this->pcpu_refcnt != NULL)
 187                this_cpu_inc(*this->pcpu_refcnt);
 188}
 189
 190int cffrml_refcnt_read(struct cflayer *layr)
 191{
 192        int i, refcnt = 0;
 193        struct cffrml *this = container_obj(layr);
 194        for_each_possible_cpu(i)
 195                refcnt += *per_cpu_ptr(this->pcpu_refcnt, i);
 196        return refcnt;
 197}
 198
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.