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 | ||
b482cd20 SB |
74 | int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid) |
75 | { | |
76 | struct cfmuxl *muxl = (struct cfmuxl *) layr; | |
77 | spin_lock(&muxl->transmit_lock); | |
78 | list_add(&dn->node, &muxl->frml_list); | |
79 | spin_unlock(&muxl->transmit_lock); | |
80 | return 0; | |
81 | } | |
82 | ||
83 | static struct cflayer *get_from_id(struct list_head *list, u16 id) | |
84 | { | |
85 | struct list_head *node; | |
86 | struct cflayer *layer; | |
87 | list_for_each(node, list) { | |
88 | layer = list_entry(node, struct cflayer, node); | |
89 | if (layer->id == id) | |
90 | return layer; | |
91 | } | |
92 | return NULL; | |
93 | } | |
94 | ||
95 | struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid) | |
96 | { | |
97 | struct cfmuxl *muxl = container_obj(layr); | |
98 | struct cflayer *dn; | |
99 | spin_lock(&muxl->transmit_lock); | |
100 | memset(muxl->dn_cache, 0, sizeof(muxl->dn_cache)); | |
101 | dn = get_from_id(&muxl->frml_list, phyid); | |
102 | if (dn == NULL) { | |
103 | spin_unlock(&muxl->transmit_lock); | |
104 | return NULL; | |
105 | } | |
106 | list_del(&dn->node); | |
107 | caif_assert(dn != NULL); | |
108 | spin_unlock(&muxl->transmit_lock); | |
109 | return dn; | |
110 | } | |
111 | ||
112 | /* Invariant: lock is taken */ | |
113 | static struct cflayer *get_up(struct cfmuxl *muxl, u16 id) | |
114 | { | |
115 | struct cflayer *up; | |
116 | int idx = id % UP_CACHE_SIZE; | |
117 | up = muxl->up_cache[idx]; | |
118 | if (up == NULL || up->id != id) { | |
119 | up = get_from_id(&muxl->srvl_list, id); | |
120 | muxl->up_cache[idx] = up; | |
121 | } | |
122 | return up; | |
123 | } | |
124 | ||
125 | /* Invariant: lock is taken */ | |
126 | static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info) | |
127 | { | |
128 | struct cflayer *dn; | |
129 | int idx = dev_info->id % DN_CACHE_SIZE; | |
130 | dn = muxl->dn_cache[idx]; | |
131 | if (dn == NULL || dn->id != dev_info->id) { | |
132 | dn = get_from_id(&muxl->frml_list, dev_info->id); | |
133 | muxl->dn_cache[idx] = dn; | |
134 | } | |
135 | return dn; | |
136 | } | |
137 | ||
138 | struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id) | |
139 | { | |
140 | struct cflayer *up; | |
141 | struct cfmuxl *muxl = container_obj(layr); | |
142 | spin_lock(&muxl->receive_lock); | |
143 | up = get_up(muxl, id); | |
5b208656 | 144 | if (up == NULL) |
a9a8f107 | 145 | goto out; |
b482cd20 SB |
146 | memset(muxl->up_cache, 0, sizeof(muxl->up_cache)); |
147 | list_del(&up->node); | |
5b208656 | 148 | cfsrvl_put(up); |
a9a8f107 | 149 | out: |
b482cd20 SB |
150 | spin_unlock(&muxl->receive_lock); |
151 | return up; | |
152 | } | |
153 | ||
154 | static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt) | |
155 | { | |
156 | int ret; | |
157 | struct cfmuxl *muxl = container_obj(layr); | |
158 | u8 id; | |
159 | struct cflayer *up; | |
160 | if (cfpkt_extr_head(pkt, &id, 1) < 0) { | |
b31fa5ba | 161 | pr_err("erroneous Caif Packet\n"); |
b482cd20 SB |
162 | cfpkt_destroy(pkt); |
163 | return -EPROTO; | |
164 | } | |
165 | ||
166 | spin_lock(&muxl->receive_lock); | |
167 | up = get_up(muxl, id); | |
168 | spin_unlock(&muxl->receive_lock); | |
169 | if (up == NULL) { | |
b31fa5ba JP |
170 | pr_info("Received data on unknown link ID = %d (0x%x) up == NULL", |
171 | id, id); | |
b482cd20 SB |
172 | cfpkt_destroy(pkt); |
173 | /* | |
174 | * Don't return ERROR, since modem misbehaves and sends out | |
175 | * flow on before linksetup response. | |
176 | */ | |
177 | return /* CFGLU_EPROT; */ 0; | |
178 | } | |
5b208656 | 179 | cfsrvl_get(up); |
b482cd20 | 180 | ret = up->receive(up, pkt); |
5b208656 | 181 | cfsrvl_put(up); |
b482cd20 SB |
182 | return ret; |
183 | } | |
184 | ||
185 | static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt) | |
186 | { | |
b482cd20 SB |
187 | struct cfmuxl *muxl = container_obj(layr); |
188 | u8 linkid; | |
189 | struct cflayer *dn; | |
190 | struct caif_payload_info *info = cfpkt_info(pkt); | |
39b9afbb SB |
191 | BUG_ON(!info); |
192 | dn = get_dn(muxl, info->dev_info); | |
b482cd20 | 193 | if (dn == NULL) { |
b31fa5ba JP |
194 | pr_warn("Send data on unknown phy ID = %d (0x%x)\n", |
195 | info->dev_info->id, info->dev_info->id); | |
b482cd20 SB |
196 | return -ENOTCONN; |
197 | } | |
198 | info->hdr_len += 1; | |
199 | linkid = info->channel_id; | |
200 | cfpkt_add_head(pkt, &linkid, 1); | |
4dd820c0 | 201 | return dn->transmit(dn, pkt); |
b482cd20 SB |
202 | } |
203 | ||
204 | static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, | |
205 | int phyid) | |
206 | { | |
207 | struct cfmuxl *muxl = container_obj(layr); | |
0c184ed9 | 208 | struct list_head *node, *next; |
b482cd20 | 209 | struct cflayer *layer; |
0c184ed9 | 210 | list_for_each_safe(node, next, &muxl->srvl_list) { |
b482cd20 SB |
211 | layer = list_entry(node, struct cflayer, node); |
212 | if (cfsrvl_phyid_match(layer, phyid)) | |
213 | layer->ctrlcmd(layer, ctrl, phyid); | |
214 | } | |
215 | } |