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
7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
9 #include <linux/stddef.h>
10 #include <linux/spinlock.h>
11 #include <linux/slab.h>
12 #include <linux/rculist.h>
13 #include <net/caif/cfpkt.h>
14 #include <net/caif/cfmuxl.h>
15 #include <net/caif/cfsrvl.h>
16 #include <net/caif/cffrml.h>
18 #define container_obj(layr) container_of(layr, struct cfmuxl, layer)
20 #define CAIF_CTRL_CHANNEL 0
21 #define UP_CACHE_SIZE 8
22 #define DN_CACHE_SIZE 8
26 struct list_head srvl_list
;
27 struct list_head frml_list
;
28 struct cflayer
*up_cache
[UP_CACHE_SIZE
];
29 struct cflayer
*dn_cache
[DN_CACHE_SIZE
];
31 * Set when inserting or removing downwards layers.
33 spinlock_t transmit_lock
;
36 * Set when inserting or removing upwards layers.
38 spinlock_t receive_lock
;
42 static int cfmuxl_receive(struct cflayer
*layr
, struct cfpkt
*pkt
);
43 static int cfmuxl_transmit(struct cflayer
*layr
, struct cfpkt
*pkt
);
44 static void cfmuxl_ctrlcmd(struct cflayer
*layr
, enum caif_ctrlcmd ctrl
,
46 static struct cflayer
*get_up(struct cfmuxl
*muxl
, u16 id
);
48 struct cflayer
*cfmuxl_create(void)
50 struct cfmuxl
*this = kmalloc(sizeof(struct cfmuxl
), GFP_ATOMIC
);
53 memset(this, 0, sizeof(*this));
54 this->layer
.receive
= cfmuxl_receive
;
55 this->layer
.transmit
= cfmuxl_transmit
;
56 this->layer
.ctrlcmd
= cfmuxl_ctrlcmd
;
57 INIT_LIST_HEAD(&this->srvl_list
);
58 INIT_LIST_HEAD(&this->frml_list
);
59 spin_lock_init(&this->transmit_lock
);
60 spin_lock_init(&this->receive_lock
);
61 snprintf(this->layer
.name
, CAIF_LAYER_NAME_SZ
, "mux");
65 int cfmuxl_set_uplayer(struct cflayer
*layr
, struct cflayer
*up
, u8 linkid
)
67 struct cfmuxl
*muxl
= container_obj(layr
);
69 spin_lock_bh(&muxl
->receive_lock
);
70 list_add_rcu(&up
->node
, &muxl
->srvl_list
);
71 spin_unlock_bh(&muxl
->receive_lock
);
75 int cfmuxl_set_dnlayer(struct cflayer
*layr
, struct cflayer
*dn
, u8 phyid
)
77 struct cfmuxl
*muxl
= (struct cfmuxl
*) layr
;
79 spin_lock_bh(&muxl
->transmit_lock
);
80 list_add_rcu(&dn
->node
, &muxl
->frml_list
);
81 spin_unlock_bh(&muxl
->transmit_lock
);
85 static struct cflayer
*get_from_id(struct list_head
*list
, u16 id
)
88 list_for_each_entry_rcu(lyr
, list
, node
) {
96 struct cflayer
*cfmuxl_remove_dnlayer(struct cflayer
*layr
, u8 phyid
)
98 struct cfmuxl
*muxl
= container_obj(layr
);
100 int idx
= phyid
% DN_CACHE_SIZE
;
102 spin_lock_bh(&muxl
->transmit_lock
);
103 rcu_assign_pointer(muxl
->dn_cache
[idx
], NULL
);
104 dn
= get_from_id(&muxl
->frml_list
, phyid
);
108 list_del_rcu(&dn
->node
);
109 caif_assert(dn
!= NULL
);
111 spin_unlock_bh(&muxl
->transmit_lock
);
115 static struct cflayer
*get_up(struct cfmuxl
*muxl
, u16 id
)
118 int idx
= id
% UP_CACHE_SIZE
;
119 up
= rcu_dereference(muxl
->up_cache
[idx
]);
120 if (up
== NULL
|| up
->id
!= id
) {
121 spin_lock_bh(&muxl
->receive_lock
);
122 up
= get_from_id(&muxl
->srvl_list
, id
);
123 rcu_assign_pointer(muxl
->up_cache
[idx
], up
);
124 spin_unlock_bh(&muxl
->receive_lock
);
129 static struct cflayer
*get_dn(struct cfmuxl
*muxl
, struct dev_info
*dev_info
)
132 int idx
= dev_info
->id
% DN_CACHE_SIZE
;
133 dn
= rcu_dereference(muxl
->dn_cache
[idx
]);
134 if (dn
== NULL
|| dn
->id
!= dev_info
->id
) {
135 spin_lock_bh(&muxl
->transmit_lock
);
136 dn
= get_from_id(&muxl
->frml_list
, dev_info
->id
);
137 rcu_assign_pointer(muxl
->dn_cache
[idx
], dn
);
138 spin_unlock_bh(&muxl
->transmit_lock
);
143 struct cflayer
*cfmuxl_remove_uplayer(struct cflayer
*layr
, u8 id
)
146 struct cfmuxl
*muxl
= container_obj(layr
);
147 int idx
= id
% UP_CACHE_SIZE
;
149 spin_lock_bh(&muxl
->receive_lock
);
150 up
= get_from_id(&muxl
->srvl_list
, id
);
154 rcu_assign_pointer(muxl
->up_cache
[idx
], NULL
);
155 list_del_rcu(&up
->node
);
157 spin_unlock_bh(&muxl
->receive_lock
);
161 static int cfmuxl_receive(struct cflayer
*layr
, struct cfpkt
*pkt
)
164 struct cfmuxl
*muxl
= container_obj(layr
);
167 if (cfpkt_extr_head(pkt
, &id
, 1) < 0) {
168 pr_err("erroneous Caif Packet\n");
173 up
= get_up(muxl
, id
);
176 pr_debug("Received data on unknown link ID = %d (0x%x)"
177 " up == NULL", id
, id
);
180 * Don't return ERROR, since modem misbehaves and sends out
181 * flow on before linksetup response.
185 return /* CFGLU_EPROT; */ 0;
188 /* We can't hold rcu_lock during receive, so take a ref count instead */
192 ret
= up
->receive(up
, pkt
);
198 static int cfmuxl_transmit(struct cflayer
*layr
, struct cfpkt
*pkt
)
200 struct cfmuxl
*muxl
= container_obj(layr
);
204 struct caif_payload_info
*info
= cfpkt_info(pkt
);
209 dn
= get_dn(muxl
, info
->dev_info
);
211 pr_debug("Send data on unknown phy ID = %d (0x%x)\n",
212 info
->dev_info
->id
, info
->dev_info
->id
);
219 linkid
= info
->channel_id
;
220 cfpkt_add_head(pkt
, &linkid
, 1);
222 /* We can't hold rcu_lock during receive, so take a ref count instead */
227 err
= dn
->transmit(dn
, pkt
);
233 static void cfmuxl_ctrlcmd(struct cflayer
*layr
, enum caif_ctrlcmd ctrl
,
236 struct cfmuxl
*muxl
= container_obj(layr
);
237 struct cflayer
*layer
;
240 list_for_each_entry_rcu(layer
, &muxl
->srvl_list
, node
) {
241 if (cfsrvl_phyid_match(layer
, phyid
) && layer
->ctrlcmd
)
242 /* NOTE: ctrlcmd is not allowed to block */
243 layer
->ctrlcmd(layer
, ctrl
, phyid
);
This page took 0.036509 seconds and 6 git commands to generate.