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 19static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt); 20static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt); 21 22struct 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 32static 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 43struct 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 72static 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 98static 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 170out: 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 194static 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 211static 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; 280out: 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} 311