Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 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 | |
Joe Perches | b31fa5b | 2010-09-05 21:31:11 +0000 | [diff] [blame] | 7 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
| 8 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 9 | #include <linux/kernel.h> |
| 10 | #include <linux/types.h> |
| 11 | #include <linux/errno.h> |
| 12 | #include <linux/slab.h> |
sjur.brandeland@stericsson.com | 43e3692 | 2011-05-13 02:44:03 +0000 | [diff] [blame] | 13 | #include <linux/module.h> |
Dmitry Tarnyagin | 4476481 | 2012-04-12 08:27:24 +0000 | [diff] [blame^] | 14 | #include <linux/pkt_sched.h> |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 15 | #include <net/caif/caif_layer.h> |
| 16 | #include <net/caif/cfsrvl.h> |
| 17 | #include <net/caif/cfpkt.h> |
| 18 | |
| 19 | #define SRVL_CTRL_PKT_SIZE 1 |
| 20 | #define SRVL_FLOW_OFF 0x81 |
| 21 | #define SRVL_FLOW_ON 0x80 |
| 22 | #define SRVL_SET_PIN 0x82 |
| 23 | #define SRVL_CTRL_PKT_SIZE 1 |
| 24 | |
| 25 | #define container_obj(layr) container_of(layr, struct cfsrvl, layer) |
| 26 | |
| 27 | static void cfservl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, |
| 28 | int phyid) |
| 29 | { |
| 30 | struct cfsrvl *service = container_obj(layr); |
Sjur Braendeland | b1c7424 | 2010-06-17 06:55:38 +0000 | [diff] [blame] | 31 | |
sjur.brandeland@stericsson.com | 43e3692 | 2011-05-13 02:44:03 +0000 | [diff] [blame] | 32 | if (layr->up == NULL || layr->up->ctrlcmd == NULL) |
| 33 | return; |
Sjur Braendeland | b1c7424 | 2010-06-17 06:55:38 +0000 | [diff] [blame] | 34 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 35 | switch (ctrl) { |
| 36 | case CAIF_CTRLCMD_INIT_RSP: |
| 37 | service->open = true; |
| 38 | layr->up->ctrlcmd(layr->up, ctrl, phyid); |
| 39 | break; |
| 40 | case CAIF_CTRLCMD_DEINIT_RSP: |
| 41 | case CAIF_CTRLCMD_INIT_FAIL_RSP: |
| 42 | service->open = false; |
| 43 | layr->up->ctrlcmd(layr->up, ctrl, phyid); |
| 44 | break; |
| 45 | case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND: |
| 46 | if (phyid != service->dev_info.id) |
| 47 | break; |
| 48 | if (service->modem_flow_on) |
| 49 | layr->up->ctrlcmd(layr->up, |
| 50 | CAIF_CTRLCMD_FLOW_OFF_IND, phyid); |
| 51 | service->phy_flow_on = false; |
| 52 | break; |
| 53 | case _CAIF_CTRLCMD_PHYIF_FLOW_ON_IND: |
| 54 | if (phyid != service->dev_info.id) |
| 55 | return; |
| 56 | if (service->modem_flow_on) { |
| 57 | layr->up->ctrlcmd(layr->up, |
| 58 | CAIF_CTRLCMD_FLOW_ON_IND, |
| 59 | phyid); |
| 60 | } |
| 61 | service->phy_flow_on = true; |
| 62 | break; |
| 63 | case CAIF_CTRLCMD_FLOW_OFF_IND: |
| 64 | if (service->phy_flow_on) { |
| 65 | layr->up->ctrlcmd(layr->up, |
| 66 | CAIF_CTRLCMD_FLOW_OFF_IND, phyid); |
| 67 | } |
| 68 | service->modem_flow_on = false; |
| 69 | break; |
| 70 | case CAIF_CTRLCMD_FLOW_ON_IND: |
| 71 | if (service->phy_flow_on) { |
| 72 | layr->up->ctrlcmd(layr->up, |
| 73 | CAIF_CTRLCMD_FLOW_ON_IND, phyid); |
| 74 | } |
| 75 | service->modem_flow_on = true; |
| 76 | break; |
| 77 | case _CAIF_CTRLCMD_PHYIF_DOWN_IND: |
| 78 | /* In case interface is down, let's fake a remove shutdown */ |
| 79 | layr->up->ctrlcmd(layr->up, |
| 80 | CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND, phyid); |
| 81 | break; |
| 82 | case CAIF_CTRLCMD_REMOTE_SHUTDOWN_IND: |
| 83 | layr->up->ctrlcmd(layr->up, ctrl, phyid); |
| 84 | break; |
| 85 | default: |
Joe Perches | b31fa5b | 2010-09-05 21:31:11 +0000 | [diff] [blame] | 86 | pr_warn("Unexpected ctrl in cfsrvl (%d)\n", ctrl); |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 87 | /* We have both modem and phy flow on, send flow on */ |
| 88 | layr->up->ctrlcmd(layr->up, ctrl, phyid); |
| 89 | service->phy_flow_on = true; |
| 90 | break; |
| 91 | } |
| 92 | } |
| 93 | |
| 94 | static int cfservl_modemcmd(struct cflayer *layr, enum caif_modemcmd ctrl) |
| 95 | { |
| 96 | struct cfsrvl *service = container_obj(layr); |
Sjur Braendeland | b1c7424 | 2010-06-17 06:55:38 +0000 | [diff] [blame] | 97 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 98 | caif_assert(layr != NULL); |
| 99 | caif_assert(layr->dn != NULL); |
| 100 | caif_assert(layr->dn->transmit != NULL); |
Sjur Braendeland | b1c7424 | 2010-06-17 06:55:38 +0000 | [diff] [blame] | 101 | |
| 102 | if (!service->supports_flowctrl) |
| 103 | return 0; |
| 104 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 105 | switch (ctrl) { |
| 106 | case CAIF_MODEMCMD_FLOW_ON_REQ: |
| 107 | { |
| 108 | struct cfpkt *pkt; |
| 109 | struct caif_payload_info *info; |
| 110 | u8 flow_on = SRVL_FLOW_ON; |
| 111 | pkt = cfpkt_create(SRVL_CTRL_PKT_SIZE); |
Joe Perches | 7ac2ed0 | 2011-08-25 13:22:24 +0000 | [diff] [blame] | 112 | if (!pkt) |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 113 | return -ENOMEM; |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 114 | |
| 115 | if (cfpkt_add_head(pkt, &flow_on, 1) < 0) { |
Joe Perches | b31fa5b | 2010-09-05 21:31:11 +0000 | [diff] [blame] | 116 | pr_err("Packet is erroneous!\n"); |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 117 | cfpkt_destroy(pkt); |
| 118 | return -EPROTO; |
| 119 | } |
| 120 | info = cfpkt_info(pkt); |
| 121 | info->channel_id = service->layer.id; |
| 122 | info->hdr_len = 1; |
| 123 | info->dev_info = &service->dev_info; |
Dmitry Tarnyagin | 4476481 | 2012-04-12 08:27:24 +0000 | [diff] [blame^] | 124 | cfpkt_set_prio(pkt, TC_PRIO_CONTROL); |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 125 | return layr->dn->transmit(layr->dn, pkt); |
| 126 | } |
| 127 | case CAIF_MODEMCMD_FLOW_OFF_REQ: |
| 128 | { |
| 129 | struct cfpkt *pkt; |
| 130 | struct caif_payload_info *info; |
| 131 | u8 flow_off = SRVL_FLOW_OFF; |
| 132 | pkt = cfpkt_create(SRVL_CTRL_PKT_SIZE); |
Joe Perches | 7ac2ed0 | 2011-08-25 13:22:24 +0000 | [diff] [blame] | 133 | if (!pkt) |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 134 | return -ENOMEM; |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 135 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 136 | if (cfpkt_add_head(pkt, &flow_off, 1) < 0) { |
Joe Perches | b31fa5b | 2010-09-05 21:31:11 +0000 | [diff] [blame] | 137 | pr_err("Packet is erroneous!\n"); |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 138 | cfpkt_destroy(pkt); |
| 139 | return -EPROTO; |
| 140 | } |
| 141 | info = cfpkt_info(pkt); |
| 142 | info->channel_id = service->layer.id; |
| 143 | info->hdr_len = 1; |
| 144 | info->dev_info = &service->dev_info; |
Dmitry Tarnyagin | 4476481 | 2012-04-12 08:27:24 +0000 | [diff] [blame^] | 145 | cfpkt_set_prio(pkt, TC_PRIO_CONTROL); |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 146 | return layr->dn->transmit(layr->dn, pkt); |
| 147 | } |
| 148 | default: |
| 149 | break; |
| 150 | } |
| 151 | return -EINVAL; |
| 152 | } |
| 153 | |
sjur.brandeland@stericsson.com | 43e3692 | 2011-05-13 02:44:03 +0000 | [diff] [blame] | 154 | static void cfsrvl_release(struct cflayer *layer) |
Sjur Braendeland | a7da1f5 | 2010-06-17 06:55:39 +0000 | [diff] [blame] | 155 | { |
sjur.brandeland@stericsson.com | 43e3692 | 2011-05-13 02:44:03 +0000 | [diff] [blame] | 156 | struct cfsrvl *service = container_of(layer, struct cfsrvl, layer); |
Sjur Braendeland | a7da1f5 | 2010-06-17 06:55:39 +0000 | [diff] [blame] | 157 | kfree(service); |
| 158 | } |
| 159 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 160 | void cfsrvl_init(struct cfsrvl *service, |
Sjur Braendeland | b1c7424 | 2010-06-17 06:55:38 +0000 | [diff] [blame] | 161 | u8 channel_id, |
| 162 | struct dev_info *dev_info, |
| 163 | bool supports_flowctrl |
| 164 | ) |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 165 | { |
| 166 | caif_assert(offsetof(struct cfsrvl, layer) == 0); |
| 167 | service->open = false; |
| 168 | service->modem_flow_on = true; |
| 169 | service->phy_flow_on = true; |
| 170 | service->layer.id = channel_id; |
| 171 | service->layer.ctrlcmd = cfservl_ctrlcmd; |
| 172 | service->layer.modemcmd = cfservl_modemcmd; |
| 173 | service->dev_info = *dev_info; |
Sjur Braendeland | b1c7424 | 2010-06-17 06:55:38 +0000 | [diff] [blame] | 174 | service->supports_flowctrl = supports_flowctrl; |
Sjur Braendeland | a7da1f5 | 2010-06-17 06:55:39 +0000 | [diff] [blame] | 175 | service->release = cfsrvl_release; |
Sjur Braendeland | 5b20865 | 2010-04-28 08:54:36 +0000 | [diff] [blame] | 176 | } |
| 177 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 178 | bool cfsrvl_ready(struct cfsrvl *service, int *err) |
| 179 | { |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 180 | if (!service->open) { |
| 181 | *err = -ENOTCONN; |
| 182 | return false; |
| 183 | } |
Dmitry Tarnyagin | 374458b | 2012-03-11 10:28:31 +0000 | [diff] [blame] | 184 | return true; |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 185 | } |
sjur.brandeland@stericsson.com | 43e3692 | 2011-05-13 02:44:03 +0000 | [diff] [blame] | 186 | |
Sjur Braendeland | b482cd2 | 2010-03-30 13:56:23 +0000 | [diff] [blame] | 187 | u8 cfsrvl_getphyid(struct cflayer *layer) |
| 188 | { |
| 189 | struct cfsrvl *servl = container_obj(layer); |
| 190 | return servl->dev_info.id; |
| 191 | } |
| 192 | |
| 193 | bool cfsrvl_phyid_match(struct cflayer *layer, int phyid) |
| 194 | { |
| 195 | struct cfsrvl *servl = container_obj(layer); |
| 196 | return servl->dev_info.id == phyid; |
| 197 | } |
sjur.brandeland@stericsson.com | 43e3692 | 2011-05-13 02:44:03 +0000 | [diff] [blame] | 198 | |
| 199 | void caif_free_client(struct cflayer *adap_layer) |
| 200 | { |
| 201 | struct cfsrvl *servl; |
| 202 | if (adap_layer == NULL || adap_layer->dn == NULL) |
| 203 | return; |
| 204 | servl = container_obj(adap_layer->dn); |
| 205 | servl->release(&servl->layer); |
| 206 | } |
| 207 | EXPORT_SYMBOL(caif_free_client); |
| 208 | |
| 209 | void caif_client_register_refcnt(struct cflayer *adapt_layer, |
| 210 | void (*hold)(struct cflayer *lyr), |
| 211 | void (*put)(struct cflayer *lyr)) |
| 212 | { |
| 213 | struct cfsrvl *service; |
| 214 | service = container_of(adapt_layer->dn, struct cfsrvl, layer); |
| 215 | |
| 216 | WARN_ON(adapt_layer == NULL || adapt_layer->dn == NULL); |
| 217 | service->hold = hold; |
| 218 | service->put = put; |
| 219 | } |
| 220 | EXPORT_SYMBOL(caif_client_register_refcnt); |