release/src-rt-6.x.4708/linux/linux-2.6.36/net/caif/cfrfml.c

   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 #include <linux/stddef.h>
   8 #include <linux/spinlock.h>
   9 #include <linux/slab.h>
  10 #include <asm/unaligned.h>
  11 #include <net/caif/caif_layer.h>
  12 #include <net/caif/cfsrvl.h>
  13 #include <net/caif/cfpkt.h>
  14
  15 #define container_obj(layr) container_of(layr, struct cfrfml, serv.layer)
  16 #define RFM_SEGMENTATION_BIT 0x01
  17 #define RFM_HEAD_SIZE 7
  18
  19 static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt);
  20 static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt);
  21
  22 struct cfrfml {
  23         struct cfsrvl serv;
  24         struct cfpkt *incomplete_frm;
  25         int fragment_size;
  26         u8  seghead[6];
  27         u16 pdu_size;
  28         /* Protects serialized processing of packets */
  29         spinlock_t sync;
  30 };
  31
  32 static void cfrfml_release(struct kref *kref)
  33 {
  34         struct cfsrvl *srvl = container_of(kref, struct cfsrvl, ref);
  35         struct cfrfml *rfml = container_obj(&srvl->layer);
  36
  37         if (rfml->incomplete_frm)
  38                 cfpkt_destroy(rfml->incomplete_frm);
  39
  40         kfree(srvl);
  41 }
  42
  43 struct cflayer *cfrfml_create(u8 channel_id, struct dev_info *dev_info,
  44                                         int mtu_size)
  45 {
  46         int tmp;
  47         struct cfrfml *this =
  48                 kzalloc(sizeof(struct cfrfml), GFP_ATOMIC);
  49
  50         if (!this) {
  51                 pr_warning("CAIF: %s(): Out of memory\n", __func__);
  52                 return NULL;
  53         }
  54
  55         cfsrvl_init(&this->serv, channel_id, dev_info, false);
  56         this->serv.release = cfrfml_release;
  57         this->serv.layer.receive = cfrfml_receive;
  58         this->serv.layer.transmit = cfrfml_transmit;
  59
  60         /* Round down to closest multiple of 16 */
  61         tmp = (mtu_size - RFM_HEAD_SIZE - 6) / 16;
  62         tmp *= 16;
  63
  64         this->fragment_size = tmp;
  65         spin_lock_init(&this->sync);
  66         snprintf(this->serv.layer.name, CAIF_LAYER_NAME_SZ,
  67                 "rfm%d", channel_id);
  68
  69         return &this->serv.layer;
  70 }
  71
  72 static struct cfpkt *rfm_append(struct cfrfml *rfml, char *seghead,
  73                         struct cfpkt *pkt, int *err)
  74 {
  75         struct cfpkt *tmppkt;
  76         *err = -EPROTO;
  77         /* n-th but not last segment */
  78
  79         if (cfpkt_extr_head(pkt, seghead, 6) < 0)
  80                 return NULL;
  81
  82         /* Verify correct header */
  83         if (memcmp(seghead, rfml->seghead, 6) != 0)
  84                 return NULL;
  85
  86         tmppkt = cfpkt_append(rfml->incomplete_frm, pkt,
  87                         rfml->pdu_size + RFM_HEAD_SIZE);
  88
  89         /* If cfpkt_append failes input pkts are not freed */
  90         *err = -ENOMEM;
  91         if (tmppkt == NULL)
  92                 return NULL;
  93
  94         *err = 0;
  95         return tmppkt;
  96 }
  97
  98 static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt)
  99 {
 100         u8 tmp;
 101         bool segmented;
 102         int err;
 103         u8 seghead[6];
 104         struct cfrfml *rfml;
 105         struct cfpkt *tmppkt = NULL;
 106
 107         caif_assert(layr->up != NULL);
 108         caif_assert(layr->receive != NULL);
 109         rfml = container_obj(layr);
 110         spin_lock(&rfml->sync);
 111
 112         err = -EPROTO;
 113         if (cfpkt_extr_head(pkt, &tmp, 1) < 0)
 114                 goto out;
 115         segmented = tmp & RFM_SEGMENTATION_BIT;
 116
 117         if (segmented) {
 118                 if (rfml->incomplete_frm == NULL) {
 119                         /* Initial Segment */
 120                         if (cfpkt_peek_head(pkt, rfml->seghead, 6) < 0)
 121                                 goto out;
 122
 123                         rfml->pdu_size = get_unaligned_le16(rfml->seghead+4);
 124
 125                         if (cfpkt_erroneous(pkt))
 126                                 goto out;
 127                         rfml->incomplete_frm = pkt;
 128                         pkt = NULL;
 129                 } else {
 130
 131                         tmppkt = rfm_append(rfml, seghead, pkt, &err);
 132                         if (tmppkt == NULL)
 133                                 goto out;
 134
 135                         if (cfpkt_erroneous(tmppkt))
 136                                 goto out;
 137
 138                         rfml->incomplete_frm = tmppkt;
 139
 140
 141                         if (cfpkt_erroneous(tmppkt))
 142                                 goto out;
 143                 }
 144                 err = 0;
 145                 goto out;
 146         }
 147
 148         if (rfml->incomplete_frm) {
 149
 150                 /* Last Segment */
 151                 tmppkt = rfm_append(rfml, seghead, pkt, &err);
 152                 if (tmppkt == NULL)
 153                         goto out;
 154
 155                 if (cfpkt_erroneous(tmppkt))
 156                         goto out;
 157
 158                 rfml->incomplete_frm = NULL;
 159                 pkt = tmppkt;
 160                 tmppkt = NULL;
 161
 162                 /* Verify that length is correct */
 163                 err = EPROTO;
 164                 if (rfml->pdu_size != cfpkt_getlen(pkt) - RFM_HEAD_SIZE + 1)
 165                         goto out;
 166         }
 167
 168         err = rfml->serv.layer.up->receive(rfml->serv.layer.up, pkt);
 169
 170 out:
 171
 172         if (err != 0) {
 173                 if (tmppkt)
 174                         cfpkt_destroy(tmppkt);
 175                 if (pkt)
 176                         cfpkt_destroy(pkt);
 177                 if (rfml->incomplete_frm)
 178                         cfpkt_destroy(rfml->incomplete_frm);
 179                 rfml->incomplete_frm = NULL;
 180
 181                 pr_info("CAIF: %s(): "
 182                                 "Connection error %d triggered on RFM link\n",
 183                                 __func__, err);
 184
 185                 /* Trigger connection error upon failure.*/
 186                 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
 187                                         rfml->serv.dev_info.id);
 188         }
 189         spin_unlock(&rfml->sync);
 190         return err;
 191 }
 192
 193
 194 static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt)
 195 {
 196         caif_assert(cfpkt_getlen(pkt) >= rfml->fragment_size);
 197
 198         /* Add info for MUX-layer to route the packet out. */
 199         cfpkt_info(pkt)->channel_id = rfml->serv.layer.id;
 200
 201         /*
 202          * To optimize alignment, we add up the size of CAIF header before
 203          * payload.
 204          */
 205         cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE;
 206         cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info;
 207
 208         return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt);
 209 }
 210
 211 static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt)
 212 {
 213         int err;
 214         u8 seg;
 215         u8 head[6];
 216         struct cfpkt *rearpkt = NULL;
 217         struct cfpkt *frontpkt = pkt;
 218         struct cfrfml *rfml = container_obj(layr);
 219
 220         caif_assert(layr->dn != NULL);
 221         caif_assert(layr->dn->transmit != NULL);
 222
 223         if (!cfsrvl_ready(&rfml->serv, &err))
 224                 return err;
 225
 226         err = -EPROTO;
 227         if (cfpkt_getlen(pkt) <= RFM_HEAD_SIZE-1)
 228                 goto out;
 229
 230         err = 0;
 231         if (cfpkt_getlen(pkt) > rfml->fragment_size + RFM_HEAD_SIZE)
 232                 err = cfpkt_peek_head(pkt, head, 6);
 233
 234         if (err < 0)
 235                 goto out;
 236
 237         while (cfpkt_getlen(frontpkt) > rfml->fragment_size + RFM_HEAD_SIZE) {
 238
 239                 seg = 1;
 240                 err = -EPROTO;
 241
 242                 if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
 243                         goto out;
 244                 /*
 245                  * On OOM error cfpkt_split returns NULL.
 246                  *
 247                  * NOTE: Segmented pdu is not correctly aligned.
 248                  * This has negative performance impact.
 249                  */
 250
 251                 rearpkt = cfpkt_split(frontpkt, rfml->fragment_size);
 252                 if (rearpkt == NULL)
 253                         goto out;
 254
 255                 err = cfrfml_transmit_segment(rfml, frontpkt);
 256
 257                 if (err != 0)
 258                         goto out;
 259                 frontpkt = rearpkt;
 260                 rearpkt = NULL;
 261
 262                 err = -ENOMEM;
 263                 if (frontpkt == NULL)
 264                         goto out;
 265                 err = -EPROTO;
 266                 if (cfpkt_add_head(frontpkt, head, 6) < 0)
 267                         goto out;
 268
 269         }
 270
 271         seg = 0;
 272         err = -EPROTO;
 273
 274         if (cfpkt_add_head(frontpkt, &seg, 1) < 0)
 275                 goto out;
 276
 277         err = cfrfml_transmit_segment(rfml, frontpkt);
 278
 279         frontpkt = NULL;
 280 out:
 281
 282         if (err != 0) {
 283                 pr_info("CAIF: %s(): "
 284                                 "Connection error %d triggered on RFM link\n",
 285                                 __func__, err);
 286                 /* Trigger connection error upon failure.*/
 287
 288                 layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND,
 289                                         rfml->serv.dev_info.id);
 290
 291                 if (rearpkt)
 292                         cfpkt_destroy(rearpkt);
 293
 294                 if (frontpkt && frontpkt != pkt) {
 295
 296                         cfpkt_destroy(frontpkt);
 297                         /*
 298                          * Socket layer will free the original packet,
 299                          * but this packet may already be sent and
 300                          * freed. So we have to return 0 in this case
 301                          * to avoid socket layer to re-free this packet.
 302                          * The return of shutdown indication will
 303                          * cause connection to be invalidated anyhow.
 304                          */
 305                         err = 0;
 306                 }
 307         }
 308
 309         return err;
 310 }