Blame view
net/caif/cfrfml.c
6.29 KB
af873fcec treewide: Replace... |
1 |
// SPDX-License-Identifier: GPL-2.0-only |
b482cd205 net-caif: add CAI... |
2 3 |
/* * Copyright (C) ST-Ericsson AB 2010 |
26ee65e68 caif: Remove my b... |
4 |
* Author: Sjur Brendeland |
b482cd205 net-caif: add CAI... |
5 |
*/ |
b31fa5bad net/caif: Use pr_fmt |
6 |
#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
b482cd205 net-caif: add CAI... |
7 8 9 |
#include <linux/stddef.h> #include <linux/spinlock.h> #include <linux/slab.h> |
7e368739e net/caif/cfrfml.c... |
10 |
#include <asm/unaligned.h> |
b482cd205 net-caif: add CAI... |
11 12 13 |
#include <net/caif/caif_layer.h> #include <net/caif/cfsrvl.h> #include <net/caif/cfpkt.h> |
a7da1f55a caif: Bugfix - RF... |
14 |
#define container_obj(layr) container_of(layr, struct cfrfml, serv.layer) |
b482cd205 net-caif: add CAI... |
15 |
#define RFM_SEGMENTATION_BIT 0x01 |
a7da1f55a caif: Bugfix - RF... |
16 |
#define RFM_HEAD_SIZE 7 |
b482cd205 net-caif: add CAI... |
17 18 19 |
static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt); static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt); |
b482cd205 net-caif: add CAI... |
20 |
|
a7da1f55a caif: Bugfix - RF... |
21 22 23 24 25 26 27 28 29 |
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; }; |
43e369210 caif: Move refcou... |
30 |
static void cfrfml_release(struct cflayer *layer) |
a7da1f55a caif: Bugfix - RF... |
31 |
{ |
43e369210 caif: Move refcou... |
32 |
struct cfsrvl *srvl = container_of(layer, struct cfsrvl, layer); |
a7da1f55a caif: Bugfix - RF... |
33 34 35 36 37 38 39 40 41 |
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, |
3bffc475f CAIF: fix indenta... |
42 |
int mtu_size) |
b482cd205 net-caif: add CAI... |
43 |
{ |
a7da1f55a caif: Bugfix - RF... |
44 |
int tmp; |
7ac2ed0ce caif: Remove OOM ... |
45 |
struct cfrfml *this = kzalloc(sizeof(struct cfrfml), GFP_ATOMIC); |
b1c74247b caif: Bugfix not ... |
46 |
|
7ac2ed0ce caif: Remove OOM ... |
47 |
if (!this) |
b482cd205 net-caif: add CAI... |
48 |
return NULL; |
b1c74247b caif: Bugfix not ... |
49 |
|
a7da1f55a caif: Bugfix - RF... |
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 |
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, |
3bffc475f CAIF: fix indenta... |
68 |
struct cfpkt *pkt, int *err) |
a7da1f55a caif: Bugfix - RF... |
69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 |
{ 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; |
b1c74247b caif: Bugfix not ... |
88 |
|
a7da1f55a caif: Bugfix - RF... |
89 90 |
*err = 0; return tmppkt; |
b482cd205 net-caif: add CAI... |
91 |
} |
b482cd205 net-caif: add CAI... |
92 93 94 95 |
static int cfrfml_receive(struct cflayer *layr, struct cfpkt *pkt) { u8 tmp; bool segmented; |
a7da1f55a caif: Bugfix - RF... |
96 97 98 99 |
int err; u8 seghead[6]; struct cfrfml *rfml; struct cfpkt *tmppkt = NULL; |
b482cd205 net-caif: add CAI... |
100 101 |
caif_assert(layr->up != NULL); caif_assert(layr->receive != NULL); |
a7da1f55a caif: Bugfix - RF... |
102 103 104 105 106 107 108 109 110 111 112 |
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 */ |
e10468410 net: caif: fix er... |
113 |
if (cfpkt_peek_head(pkt, rfml->seghead, 6) != 0) |
a7da1f55a caif: Bugfix - RF... |
114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 |
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 */ |
449f14f01 net: caif: fix er... |
156 |
err = -EPROTO; |
a7da1f55a caif: Bugfix - RF... |
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 |
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; |
b31fa5bad net/caif: Use pr_fmt |
173 174 |
pr_info("Connection error %d triggered on RFM link ", err); |
a7da1f55a caif: Bugfix - RF... |
175 176 177 178 179 180 |
/* Trigger connection error upon failure.*/ layr->up->ctrlcmd(layr->up, CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, rfml->serv.dev_info.id); } spin_unlock(&rfml->sync); |
374458b3f caif: Fix for a r... |
181 182 183 184 |
if (unlikely(err == -EAGAIN)) /* It is not possible to recover after drop of a fragment */ err = -EIO; |
a7da1f55a caif: Bugfix - RF... |
185 186 187 188 189 190 |
return err; } static int cfrfml_transmit_segment(struct cfrfml *rfml, struct cfpkt *pkt) { |
005b0b076 caif: Bad assert ... |
191 |
caif_assert(cfpkt_getlen(pkt) < rfml->fragment_size + RFM_HEAD_SIZE); |
a7da1f55a caif: Bugfix - RF... |
192 193 194 |
/* Add info for MUX-layer to route the packet out. */ cfpkt_info(pkt)->channel_id = rfml->serv.layer.id; |
b482cd205 net-caif: add CAI... |
195 196 |
/* |
a7da1f55a caif: Bugfix - RF... |
197 198 |
* To optimize alignment, we add up the size of CAIF header before * payload. |
b482cd205 net-caif: add CAI... |
199 |
*/ |
a7da1f55a caif: Bugfix - RF... |
200 201 |
cfpkt_info(pkt)->hdr_len = RFM_HEAD_SIZE; cfpkt_info(pkt)->dev_info = &rfml->serv.dev_info; |
b482cd205 net-caif: add CAI... |
202 |
|
a7da1f55a caif: Bugfix - RF... |
203 |
return rfml->serv.layer.dn->transmit(rfml->serv.layer.dn, pkt); |
b482cd205 net-caif: add CAI... |
204 205 206 207 |
} static int cfrfml_transmit(struct cflayer *layr, struct cfpkt *pkt) { |
a7da1f55a caif: Bugfix - RF... |
208 209 210 211 212 213 |
int err; u8 seg; u8 head[6]; struct cfpkt *rearpkt = NULL; struct cfpkt *frontpkt = pkt; struct cfrfml *rfml = container_obj(layr); |
b482cd205 net-caif: add CAI... |
214 215 216 |
caif_assert(layr->dn != NULL); caif_assert(layr->dn->transmit != NULL); |
a7da1f55a caif: Bugfix - RF... |
217 |
if (!cfsrvl_ready(&rfml->serv, &err)) |
374458b3f caif: Fix for a r... |
218 |
goto out; |
a7da1f55a caif: Bugfix - RF... |
219 220 221 222 223 224 225 226 |
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); |
e10468410 net: caif: fix er... |
227 |
if (err != 0) |
a7da1f55a caif: Bugfix - RF... |
228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 |
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); |
374458b3f caif: Fix for a r... |
249 250 |
if (err != 0) { frontpkt = NULL; |
a7da1f55a caif: Bugfix - RF... |
251 |
goto out; |
374458b3f caif: Fix for a r... |
252 |
} |
a7da1f55a caif: Bugfix - RF... |
253 254 |
frontpkt = rearpkt; rearpkt = NULL; |
a7da1f55a caif: Bugfix - RF... |
255 256 257 |
err = -EPROTO; if (cfpkt_add_head(frontpkt, head, 6) < 0) goto out; |
b482cd205 net-caif: add CAI... |
258 |
|
b482cd205 net-caif: add CAI... |
259 |
} |
a7da1f55a caif: Bugfix - RF... |
260 261 262 263 264 265 266 267 268 269 270 271 272 |
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) { |
b31fa5bad net/caif: Use pr_fmt |
273 274 |
pr_info("Connection error %d triggered on RFM link ", err); |
a7da1f55a caif: Bugfix - RF... |
275 276 277 278 279 280 281 |
/* 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); |
374458b3f caif: Fix for a r... |
282 |
if (frontpkt) |
a7da1f55a caif: Bugfix - RF... |
283 |
cfpkt_destroy(frontpkt); |
b482cd205 net-caif: add CAI... |
284 |
} |
a7da1f55a caif: Bugfix - RF... |
285 |
return err; |
b482cd205 net-caif: add CAI... |
286 |
} |