cfmuxl.c revision b482cd2053e3b90a7b33a78c63cdb6badf2ec383
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 20struct 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 38static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt); 39static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt); 40static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 41 int phyid); 42static struct cflayer *get_up(struct cfmuxl *muxl, u16 id); 43 44struct 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 61int 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 list_add(&up->node, &muxl->srvl_list); 66 spin_unlock(&muxl->receive_lock); 67 return 0; 68} 69 70bool cfmuxl_is_phy_inuse(struct cflayer *layr, u8 phyid) 71{ 72 struct list_head *node; 73 struct cflayer *layer; 74 struct cfmuxl *muxl = container_obj(layr); 75 bool match = false; 76 spin_lock(&muxl->receive_lock); 77 78 list_for_each(node, &muxl->srvl_list) { 79 layer = list_entry(node, struct cflayer, node); 80 if (cfsrvl_phyid_match(layer, phyid)) { 81 match = true; 82 break; 83 } 84 85 } 86 spin_unlock(&muxl->receive_lock); 87 return match; 88} 89 90u8 cfmuxl_get_phyid(struct cflayer *layr, u8 channel_id) 91{ 92 struct cflayer *up; 93 int phyid; 94 struct cfmuxl *muxl = container_obj(layr); 95 spin_lock(&muxl->receive_lock); 96 up = get_up(muxl, channel_id); 97 if (up != NULL) 98 phyid = cfsrvl_getphyid(up); 99 else 100 phyid = 0; 101 spin_unlock(&muxl->receive_lock); 102 return phyid; 103} 104 105int cfmuxl_set_dnlayer(struct cflayer *layr, struct cflayer *dn, u8 phyid) 106{ 107 struct cfmuxl *muxl = (struct cfmuxl *) layr; 108 spin_lock(&muxl->transmit_lock); 109 list_add(&dn->node, &muxl->frml_list); 110 spin_unlock(&muxl->transmit_lock); 111 return 0; 112} 113 114static struct cflayer *get_from_id(struct list_head *list, u16 id) 115{ 116 struct list_head *node; 117 struct cflayer *layer; 118 list_for_each(node, list) { 119 layer = list_entry(node, struct cflayer, node); 120 if (layer->id == id) 121 return layer; 122 } 123 return NULL; 124} 125 126struct cflayer *cfmuxl_remove_dnlayer(struct cflayer *layr, u8 phyid) 127{ 128 struct cfmuxl *muxl = container_obj(layr); 129 struct cflayer *dn; 130 spin_lock(&muxl->transmit_lock); 131 memset(muxl->dn_cache, 0, sizeof(muxl->dn_cache)); 132 dn = get_from_id(&muxl->frml_list, phyid); 133 if (dn == NULL) { 134 spin_unlock(&muxl->transmit_lock); 135 return NULL; 136 } 137 list_del(&dn->node); 138 caif_assert(dn != NULL); 139 spin_unlock(&muxl->transmit_lock); 140 return dn; 141} 142 143/* Invariant: lock is taken */ 144static struct cflayer *get_up(struct cfmuxl *muxl, u16 id) 145{ 146 struct cflayer *up; 147 int idx = id % UP_CACHE_SIZE; 148 up = muxl->up_cache[idx]; 149 if (up == NULL || up->id != id) { 150 up = get_from_id(&muxl->srvl_list, id); 151 muxl->up_cache[idx] = up; 152 } 153 return up; 154} 155 156/* Invariant: lock is taken */ 157static struct cflayer *get_dn(struct cfmuxl *muxl, struct dev_info *dev_info) 158{ 159 struct cflayer *dn; 160 int idx = dev_info->id % DN_CACHE_SIZE; 161 dn = muxl->dn_cache[idx]; 162 if (dn == NULL || dn->id != dev_info->id) { 163 dn = get_from_id(&muxl->frml_list, dev_info->id); 164 muxl->dn_cache[idx] = dn; 165 } 166 return dn; 167} 168 169struct cflayer *cfmuxl_remove_uplayer(struct cflayer *layr, u8 id) 170{ 171 struct cflayer *up; 172 struct cfmuxl *muxl = container_obj(layr); 173 spin_lock(&muxl->receive_lock); 174 up = get_up(muxl, id); 175 memset(muxl->up_cache, 0, sizeof(muxl->up_cache)); 176 list_del(&up->node); 177 spin_unlock(&muxl->receive_lock); 178 return up; 179} 180 181static int cfmuxl_receive(struct cflayer *layr, struct cfpkt *pkt) 182{ 183 int ret; 184 struct cfmuxl *muxl = container_obj(layr); 185 u8 id; 186 struct cflayer *up; 187 if (cfpkt_extr_head(pkt, &id, 1) < 0) { 188 pr_err("CAIF: %s(): erroneous Caif Packet\n", __func__); 189 cfpkt_destroy(pkt); 190 return -EPROTO; 191 } 192 193 spin_lock(&muxl->receive_lock); 194 up = get_up(muxl, id); 195 spin_unlock(&muxl->receive_lock); 196 if (up == NULL) { 197 pr_info("CAIF: %s():Received data on unknown link ID = %d " 198 "(0x%x) up == NULL", __func__, id, id); 199 cfpkt_destroy(pkt); 200 /* 201 * Don't return ERROR, since modem misbehaves and sends out 202 * flow on before linksetup response. 203 */ 204 return /* CFGLU_EPROT; */ 0; 205 } 206 207 ret = up->receive(up, pkt); 208 return ret; 209} 210 211static int cfmuxl_transmit(struct cflayer *layr, struct cfpkt *pkt) 212{ 213 int ret; 214 struct cfmuxl *muxl = container_obj(layr); 215 u8 linkid; 216 struct cflayer *dn; 217 struct caif_payload_info *info = cfpkt_info(pkt); 218 dn = get_dn(muxl, cfpkt_info(pkt)->dev_info); 219 if (dn == NULL) { 220 pr_warning("CAIF: %s(): Send data on unknown phy " 221 "ID = %d (0x%x)\n", 222 __func__, info->dev_info->id, info->dev_info->id); 223 return -ENOTCONN; 224 } 225 info->hdr_len += 1; 226 linkid = info->channel_id; 227 cfpkt_add_head(pkt, &linkid, 1); 228 ret = dn->transmit(dn, pkt); 229 /* Remove MUX protocol header upon error. */ 230 if (ret < 0) 231 cfpkt_extr_head(pkt, &linkid, 1); 232 return ret; 233} 234 235static void cfmuxl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 236 int phyid) 237{ 238 struct cfmuxl *muxl = container_obj(layr); 239 struct list_head *node; 240 struct cflayer *layer; 241 list_for_each(node, &muxl->srvl_list) { 242 layer = list_entry(node, struct cflayer, node); 243 if (cfsrvl_phyid_match(layer, phyid)) 244 layer->ctrlcmd(layer, ctrl, phyid); 245 } 246} 247