net/caif: Use pr_fmt
[linux-2.6.git] / net / caif / cfmuxl.c
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 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8
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);
68         cfsrvl_get(up);
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);
179         if (up == NULL)
180                 goto out;
181         memset(muxl->up_cache, 0, sizeof(muxl->up_cache));
182         list_del(&up->node);
183         cfsrvl_put(up);
184 out:
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) {
196                 pr_err("erroneous Caif Packet\n");
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) {
205                 pr_info("Received data on unknown link ID = %d (0x%x) up == NULL",
206                         id, id);
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         }
214         cfsrvl_get(up);
215         ret = up->receive(up, pkt);
216         cfsrvl_put(up);
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) {
229                 pr_warn("Send data on unknown phy ID = %d (0x%x)\n",
230                         info->dev_info->id, info->dev_info->id);
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 }