Blame view
net/caif/cfmuxl.c
6.23 KB
af873fcec
|
1 |
// SPDX-License-Identifier: GPL-2.0-only |
b482cd205
|
2 3 |
/* * Copyright (C) ST-Ericsson AB 2010 |
26ee65e68
|
4 |
* Author: Sjur Brendeland |
b482cd205
|
5 |
*/ |
b31fa5bad
|
6 7 |
#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
b482cd205
|
8 9 10 |
#include <linux/stddef.h> #include <linux/spinlock.h> #include <linux/slab.h> |
0b1e9738d
|
11 |
#include <linux/rculist.h> |
b482cd205
|
12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 |
#include <net/caif/cfpkt.h> #include <net/caif/cfmuxl.h> #include <net/caif/cfsrvl.h> #include <net/caif/cffrml.h> #define container_obj(layr) container_of(layr, struct cfmuxl, layer) #define CAIF_CTRL_CHANNEL 0 #define UP_CACHE_SIZE 8 #define DN_CACHE_SIZE 8 struct cfmuxl { struct cflayer layer; struct list_head srvl_list; struct list_head frml_list; struct cflayer *up_cache[UP_CACHE_SIZE]; struct cflayer *dn_cache[DN_CACHE_SIZE]; /* * Set when inserting or removing downwards layers. */ spinlock_t transmit_lock; /* * Set when inserting or removing upwards layers. */ spinlock_t receive_lock; }; static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt); static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt); static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, |
3bffc475f
|
44 |
int phyid); |
b482cd205
|
45 46 47 48 |
static struct cflayer *get_up(struct cfmuxl *muxl, u16 id); struct cflayer *cfmuxl_create(void) { |
6ff1e1e3c
|
49 |
struct cfmuxl *this = kzalloc(sizeof(struct cfmuxl), GFP_ATOMIC); |
b482cd205
|
50 51 |
if (!this) return NULL; |
b482cd205
|
52 53 54 55 56 57 58 59 60 61 |
this->layer.receive = cfmuxl_receive; this->layer.transmit = cfmuxl_transmit; this->layer.ctrlcmd = cfmuxl_ctrlcmd; INIT_LIST_HEAD(&this->srvl_list); INIT_LIST_HEAD(&this->frml_list); spin_lock_init(&this->transmit_lock); spin_lock_init(&this->receive_lock); snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "mux"); return &this->layer; } |
b482cd205
|
62 63 64 |
int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid) { struct cfmuxl *muxl = (struct cfmuxl *) layr; |
0b1e9738d
|
65 66 67 68 |
spin_lock_bh(&muxl->transmit_lock); list_add_rcu(&dn->node, &muxl->frml_list); spin_unlock_bh(&muxl->transmit_lock); |
b482cd205
|
69 70 71 72 73 |
return 0; } static struct cflayer *get_from_id(struct list_head *list, u16 id) { |
0b1e9738d
|
74 75 76 77 |
struct cflayer *lyr; list_for_each_entry_rcu(lyr, list, node) { if (lyr->id == id) return lyr; |
b482cd205
|
78 |
} |
0b1e9738d
|
79 |
|
b482cd205
|
80 81 |
return NULL; } |
54e90fb5c
|
82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 |
int cfmuxl_set_uplayer(struct cflayer *layr, struct cflayer *up, u8 linkid) { struct cfmuxl *muxl = container_obj(layr); struct cflayer *old; spin_lock_bh(&muxl->receive_lock); /* Two entries with same id is wrong, so remove old layer from mux */ old = get_from_id(&muxl->srvl_list, linkid); if (old != NULL) list_del_rcu(&old->node); list_add_rcu(&up->node, &muxl->srvl_list); spin_unlock_bh(&muxl->receive_lock); return 0; } |
b482cd205
|
99 100 101 102 |
struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid) { struct cfmuxl *muxl = container_obj(layr); struct cflayer *dn; |
0b1e9738d
|
103 104 105 |
int idx = phyid % DN_CACHE_SIZE; spin_lock_bh(&muxl->transmit_lock); |
a9b3cd7f3
|
106 |
RCU_INIT_POINTER(muxl->dn_cache[idx], NULL); |
b482cd205
|
107 |
dn = get_from_id(&muxl->frml_list, phyid); |
0b1e9738d
|
108 109 110 111 |
if (dn == NULL) goto out; list_del_rcu(&dn->node); |
b482cd205
|
112 |
caif_assert(dn != NULL); |
0b1e9738d
|
113 114 |
out: spin_unlock_bh(&muxl->transmit_lock); |
b482cd205
|
115 116 |
return dn; } |
b482cd205
|
117 118 119 120 |
static struct cflayer *get_up(struct cfmuxl *muxl, u16 id) { struct cflayer *up; int idx = id % UP_CACHE_SIZE; |
0b1e9738d
|
121 |
up = rcu_dereference(muxl->up_cache[idx]); |
b482cd205
|
122 |
if (up == NULL || up->id != id) { |
0b1e9738d
|
123 |
spin_lock_bh(&muxl->receive_lock); |
b482cd205
|
124 |
up = get_from_id(&muxl->srvl_list, id); |
0b1e9738d
|
125 126 |
rcu_assign_pointer(muxl->up_cache[idx], up); spin_unlock_bh(&muxl->receive_lock); |
b482cd205
|
127 128 129 |
} return up; } |
b482cd205
|
130 131 132 133 |
static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info) { struct cflayer *dn; int idx = dev_info->id % DN_CACHE_SIZE; |
0b1e9738d
|
134 |
dn = rcu_dereference(muxl->dn_cache[idx]); |
b482cd205
|
135 |
if (dn == NULL || dn->id != dev_info->id) { |
0b1e9738d
|
136 |
spin_lock_bh(&muxl->transmit_lock); |
b482cd205
|
137 |
dn = get_from_id(&muxl->frml_list, dev_info->id); |
0b1e9738d
|
138 139 |
rcu_assign_pointer(muxl->dn_cache[idx], dn); spin_unlock_bh(&muxl->transmit_lock); |
b482cd205
|
140 141 142 143 144 145 146 147 |
} return dn; } struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id) { struct cflayer *up; struct cfmuxl *muxl = container_obj(layr); |
0b1e9738d
|
148 |
int idx = id % UP_CACHE_SIZE; |
54e90fb5c
|
149 150 151 152 153 |
if (id == 0) { pr_warn("Trying to remove control layer "); return NULL; } |
0b1e9738d
|
154 155 |
spin_lock_bh(&muxl->receive_lock); up = get_from_id(&muxl->srvl_list, id); |
5b2086567
|
156 |
if (up == NULL) |
a9a8f1070
|
157 |
goto out; |
0b1e9738d
|
158 |
|
a9b3cd7f3
|
159 |
RCU_INIT_POINTER(muxl->up_cache[idx], NULL); |
0b1e9738d
|
160 |
list_del_rcu(&up->node); |
a9a8f1070
|
161 |
out: |
0b1e9738d
|
162 |
spin_unlock_bh(&muxl->receive_lock); |
b482cd205
|
163 164 165 166 167 168 169 170 171 172 |
return up; } static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt) { int ret; struct cfmuxl *muxl = container_obj(layr); u8 id; struct cflayer *up; if (cfpkt_extr_head(pkt, &id, 1) < 0) { |
b31fa5bad
|
173 174 |
pr_err("erroneous Caif Packet "); |
b482cd205
|
175 176 177 |
cfpkt_destroy(pkt); return -EPROTO; } |
0b1e9738d
|
178 |
rcu_read_lock(); |
b482cd205
|
179 |
up = get_up(muxl, id); |
0b1e9738d
|
180 |
|
b482cd205
|
181 |
if (up == NULL) { |
0b1e9738d
|
182 183 |
pr_debug("Received data on unknown link ID = %d (0x%x)" " up == NULL", id, id); |
b482cd205
|
184 185 186 187 188 |
cfpkt_destroy(pkt); /* * Don't return ERROR, since modem misbehaves and sends out * flow on before linksetup response. */ |
0b1e9738d
|
189 190 |
rcu_read_unlock(); |
b482cd205
|
191 192 |
return /* CFGLU_EPROT; */ 0; } |
0b1e9738d
|
193 194 |
/* We can't hold rcu_lock during receive, so take a ref count instead */ |
5b2086567
|
195 |
cfsrvl_get(up); |
0b1e9738d
|
196 |
rcu_read_unlock(); |
b482cd205
|
197 |
ret = up->receive(up, pkt); |
0b1e9738d
|
198 |
|
5b2086567
|
199 |
cfsrvl_put(up); |
b482cd205
|
200 201 202 203 204 |
return ret; } static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt) { |
b482cd205
|
205 |
struct cfmuxl *muxl = container_obj(layr); |
0b1e9738d
|
206 |
int err; |
b482cd205
|
207 208 209 |
u8 linkid; struct cflayer *dn; struct caif_payload_info *info = cfpkt_info(pkt); |
39b9afbb4
|
210 |
BUG_ON(!info); |
0b1e9738d
|
211 212 |
rcu_read_lock(); |
39b9afbb4
|
213 |
dn = get_dn(muxl, info->dev_info); |
b482cd205
|
214 |
if (dn == NULL) { |
0b1e9738d
|
215 216 |
pr_debug("Send data on unknown phy ID = %d (0x%x) ", |
b31fa5bad
|
217 |
info->dev_info->id, info->dev_info->id); |
0b1e9738d
|
218 219 |
rcu_read_unlock(); cfpkt_destroy(pkt); |
b482cd205
|
220 221 |
return -ENOTCONN; } |
0b1e9738d
|
222 |
|
b482cd205
|
223 224 225 |
info->hdr_len += 1; linkid = info->channel_id; cfpkt_add_head(pkt, &linkid, 1); |
0b1e9738d
|
226 227 228 229 230 231 232 233 234 235 |
/* We can't hold rcu_lock during receive, so take a ref count instead */ cffrml_hold(dn); rcu_read_unlock(); err = dn->transmit(dn, pkt); cffrml_put(dn); return err; |
b482cd205
|
236 237 238 |
} static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, |
3bffc475f
|
239 |
int phyid) |
b482cd205
|
240 241 |
{ struct cfmuxl *muxl = container_obj(layr); |
b482cd205
|
242 |
struct cflayer *layer; |
0b1e9738d
|
243 244 245 |
rcu_read_lock(); list_for_each_entry_rcu(layer, &muxl->srvl_list, node) { |
54e90fb5c
|
246 247 |
if (cfsrvl_phyid_match(layer, phyid) && layer->ctrlcmd) { |
a1b7f85e4
|
248 |
if ((ctrl == _CAIF_CTRLCMD_PHYIF_DOWN_IND || |
54e90fb5c
|
249 |
ctrl == CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND) && |
b01377a42
|
250 251 |
layer->id != 0) cfmuxl_remove_uplayer(layr, layer->id); |
0b1e9738d
|
252 |
/* NOTE: ctrlcmd is not allowed to block */ |
b482cd205
|
253 |
layer->ctrlcmd(layer, ctrl, phyid); |
54e90fb5c
|
254 |
} |
b482cd205
|
255 |
} |
0b1e9738d
|
256 |
rcu_read_unlock(); |
b482cd205
|
257 |
} |