Commit | Line | Data |
---|---|---|
b482cd20 SB |
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 | */ | |
b31fa5ba JP |
6 | |
7 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ | |
8 | ||
b482cd20 SB |
9 | #include <linux/stddef.h> |
10 | #include <linux/spinlock.h> | |
11 | #include <linux/slab.h> | |
12 | #include <net/caif/cfpkt.h> | |
13 | #include <net/caif/cfmuxl.h> | |
14 | #include <net/caif/cfsrvl.h> | |
15 | #include <net/caif/cffrml.h> | |
16 | ||
17 | #define container_obj(layr) container_of(layr, struct cfmuxl, layer) | |
18 | ||
19 | #define CAIF_CTRL_CHANNEL 0 | |
20 | #define UP_CACHE_SIZE 8 | |
21 | #define DN_CACHE_SIZE 8 | |
22 | ||
23 | struct cfmuxl { | |
24 | struct cflayer layer; | |
25 | struct list_head srvl_list; | |
26 | struct list_head frml_list; | |
27 | struct cflayer *up_cache[UP_CACHE_SIZE]; | |
28 | struct cflayer *dn_cache[DN_CACHE_SIZE]; | |
29 | /* | |
30 | * Set when inserting or removing downwards layers. | |
31 | */ | |
32 | spinlock_t transmit_lock; | |
33 | ||
34 | /* | |
35 | * Set when inserting or removing upwards layers. | |
36 | */ | |
37 | spinlock_t receive_lock; | |
38 | ||
39 | }; | |
40 | ||
41 | static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt); | |
42 | static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt); | |
43 | static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
44 | int phyid); | |
45 | static struct cflayer *get_up(struct cfmuxl *muxl, u16 id); | |
46 | ||
47 | struct cflayer *cfmuxl_create(void) | |
48 | { | |
49 | struct cfmuxl *this = kmalloc(sizeof(struct cfmuxl), GFP_ATOMIC); | |
50 | if (!this) | |
51 | return NULL; | |
52 | memset(this, 0, sizeof(*this)); | |
53 | this->layer.receive = cfmuxl_receive; | |
54 | this->layer.transmit = cfmuxl_transmit; | |
55 | this->layer.ctrlcmd = cfmuxl_ctrlcmd; | |
56 | INIT_LIST_HEAD(&this->srvl_list); | |
57 | INIT_LIST_HEAD(&this->frml_list); | |
58 | spin_lock_init(&this->transmit_lock); | |
59 | spin_lock_init(&this->receive_lock); | |
60 | snprintf(this->layer.name, CAIF_LAYER_NAME_SZ, "mux"); | |
61 | return &this->layer; | |
62 | } | |
63 | ||
64 | int cfmuxl_set_uplayer(struct cflayer *layr, struct cflayer *up, u8 linkid) | |
65 | { | |
66 | struct cfmuxl *muxl = container_obj(layr); | |
67 | spin_lock(&muxl->receive_lock); | |
5b208656 | 68 | cfsrvl_get(up); |
b482cd20 SB |
69 | list_add(&up->node, &muxl->srvl_list); |
70 | spin_unlock(&muxl->receive_lock); | |
71 | return 0; | |
72 | } | |
73 | ||
74 | bool cfmuxl_is_phy_inuse(struct cflayer *layr, u8 phyid) | |
75 | { | |
76 | struct list_head *node; | |
77 | struct cflayer *layer; | |
78 | struct cfmuxl *muxl = container_obj(layr); | |
79 | bool match = false; | |
80 | spin_lock(&muxl->receive_lock); | |
81 | ||
82 | list_for_each(node, &muxl->srvl_list) { | |
83 | layer = list_entry(node, struct cflayer, node); | |
84 | if (cfsrvl_phyid_match(layer, phyid)) { | |
85 | match = true; | |
86 | break; | |
87 | } | |
88 | ||
89 | } | |
90 | spin_unlock(&muxl->receive_lock); | |
91 | return match; | |
92 | } | |
93 | ||
94 | u8 cfmuxl_get_phyid(struct cflayer *layr, u8 channel_id) | |
95 | { | |
96 | struct cflayer *up; | |
97 | int phyid; | |
98 | struct cfmuxl *muxl = container_obj(layr); | |
99 | spin_lock(&muxl->receive_lock); | |
100 | up = get_up(muxl, channel_id); | |
101 | if (up != NULL) | |
102 | phyid = cfsrvl_getphyid(up); | |
103 | else | |
104 | phyid = 0; | |
105 | spin_unlock(&muxl->receive_lock); | |
106 | return phyid; | |
107 | } | |
108 | ||
109 | int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid) | |
110 | { | |
111 | struct cfmuxl *muxl = (struct cfmuxl *) layr; | |
112 | spin_lock(&muxl->transmit_lock); | |
113 | list_add(&dn->node, &muxl->frml_list); | |
114 | spin_unlock(&muxl->transmit_lock); | |
115 | return 0; | |
116 | } | |
117 | ||
118 | static struct cflayer *get_from_id(struct list_head *list, u16 id) | |
119 | { | |
120 | struct list_head *node; | |
121 | struct cflayer *layer; | |
122 | list_for_each(node, list) { | |
123 | layer = list_entry(node, struct cflayer, node); | |
124 | if (layer->id == id) | |
125 | return layer; | |
126 | } | |
127 | return NULL; | |
128 | } | |
129 | ||
130 | struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid) | |
131 | { | |
132 | struct cfmuxl *muxl = container_obj(layr); | |
133 | struct cflayer *dn; | |
134 | spin_lock(&muxl->transmit_lock); | |
135 | memset(muxl->dn_cache, 0, sizeof(muxl->dn_cache)); | |
136 | dn = get_from_id(&muxl->frml_list, phyid); | |
137 | if (dn == NULL) { | |
138 | spin_unlock(&muxl->transmit_lock); | |
139 | return NULL; | |
140 | } | |
141 | list_del(&dn->node); | |
142 | caif_assert(dn != NULL); | |
143 | spin_unlock(&muxl->transmit_lock); | |
144 | return dn; | |
145 | } | |
146 | ||
147 | /* Invariant: lock is taken */ | |
148 | static struct cflayer *get_up(struct cfmuxl *muxl, u16 id) | |
149 | { | |
150 | struct cflayer *up; | |
151 | int idx = id % UP_CACHE_SIZE; | |
152 | up = muxl->up_cache[idx]; | |
153 | if (up == NULL || up->id != id) { | |
154 | up = get_from_id(&muxl->srvl_list, id); | |
155 | muxl->up_cache[idx] = up; | |
156 | } | |
157 | return up; | |
158 | } | |
159 | ||
160 | /* Invariant: lock is taken */ | |
161 | static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info) | |
162 | { | |
163 | struct cflayer *dn; | |
164 | int idx = dev_info->id % DN_CACHE_SIZE; | |
165 | dn = muxl->dn_cache[idx]; | |
166 | if (dn == NULL || dn->id != dev_info->id) { | |
167 | dn = get_from_id(&muxl->frml_list, dev_info->id); | |
168 | muxl->dn_cache[idx] = dn; | |
169 | } | |
170 | return dn; | |
171 | } | |
172 | ||
173 | struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id) | |
174 | { | |
175 | struct cflayer *up; | |
176 | struct cfmuxl *muxl = container_obj(layr); | |
177 | spin_lock(&muxl->receive_lock); | |
178 | up = get_up(muxl, id); | |
5b208656 | 179 | if (up == NULL) |
a9a8f107 | 180 | goto out; |
b482cd20 SB |
181 | memset(muxl->up_cache, 0, sizeof(muxl->up_cache)); |
182 | list_del(&up->node); | |
5b208656 | 183 | cfsrvl_put(up); |
a9a8f107 | 184 | out: |
b482cd20 SB |
185 | spin_unlock(&muxl->receive_lock); |
186 | return up; | |
187 | } | |
188 | ||
189 | static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt) | |
190 | { | |
191 | int ret; | |
192 | struct cfmuxl *muxl = container_obj(layr); | |
193 | u8 id; | |
194 | struct cflayer *up; | |
195 | if (cfpkt_extr_head(pkt, &id, 1) < 0) { | |
b31fa5ba | 196 | pr_err("erroneous Caif Packet\n"); |
b482cd20 SB |
197 | cfpkt_destroy(pkt); |
198 | return -EPROTO; | |
199 | } | |
200 | ||
201 | spin_lock(&muxl->receive_lock); | |
202 | up = get_up(muxl, id); | |
203 | spin_unlock(&muxl->receive_lock); | |
204 | if (up == NULL) { | |
b31fa5ba JP |
205 | pr_info("Received data on unknown link ID = %d (0x%x) up == NULL", |
206 | id, id); | |
b482cd20 SB |
207 | cfpkt_destroy(pkt); |
208 | /* | |
209 | * Don't return ERROR, since modem misbehaves and sends out | |
210 | * flow on before linksetup response. | |
211 | */ | |
212 | return /* CFGLU_EPROT; */ 0; | |
213 | } | |
5b208656 | 214 | cfsrvl_get(up); |
b482cd20 | 215 | ret = up->receive(up, pkt); |
5b208656 | 216 | cfsrvl_put(up); |
b482cd20 SB |
217 | return ret; |
218 | } | |
219 | ||
220 | static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt) | |
221 | { | |
222 | int ret; | |
223 | struct cfmuxl *muxl = container_obj(layr); | |
224 | u8 linkid; | |
225 | struct cflayer *dn; | |
226 | struct caif_payload_info *info = cfpkt_info(pkt); | |
227 | dn = get_dn(muxl, cfpkt_info(pkt)->dev_info); | |
228 | if (dn == NULL) { | |
b31fa5ba JP |
229 | pr_warn("Send data on unknown phy ID = %d (0x%x)\n", |
230 | info->dev_info->id, info->dev_info->id); | |
b482cd20 SB |
231 | return -ENOTCONN; |
232 | } | |
233 | info->hdr_len += 1; | |
234 | linkid = info->channel_id; | |
235 | cfpkt_add_head(pkt, &linkid, 1); | |
236 | ret = dn->transmit(dn, pkt); | |
237 | /* Remove MUX protocol header upon error. */ | |
238 | if (ret < 0) | |
239 | cfpkt_extr_head(pkt, &linkid, 1); | |
240 | return ret; | |
241 | } | |
242 | ||
243 | static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
244 | int phyid) | |
245 | { | |
246 | struct cfmuxl *muxl = container_obj(layr); | |
247 | struct list_head *node; | |
248 | struct cflayer *layer; | |
249 | list_for_each(node, &muxl->srvl_list) { | |
250 | layer = list_entry(node, struct cflayer, node); | |
251 | if (cfsrvl_phyid_match(layer, phyid)) | |
252 | layer->ctrlcmd(layer, ctrl, phyid); | |
253 | } | |
254 | } |