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 } 255