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