1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) ST-Ericsson AB 2010 |
4 | * Author: Sjur Brendeland |
5 | */ |
6 | |
7 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
8 | |
9 | #include <linux/kernel.h> |
10 | #include <linux/types.h> |
11 | #include <linux/errno.h> |
12 | #include <linux/slab.h> |
13 | #include <linux/module.h> |
14 | #include <linux/pkt_sched.h> |
15 | #include <net/caif/caif_layer.h> |
16 | #include <net/caif/cfsrvl.h> |
17 | #include <net/caif/cfpkt.h> |
18 | #include <net/caif/caif_dev.h> |
19 | |
20 | #define SRVL_CTRL_PKT_SIZE 1 |
21 | #define SRVL_FLOW_OFF 0x81 |
22 | #define SRVL_FLOW_ON 0x80 |
23 | #define SRVL_SET_PIN 0x82 |
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); |
31 | |
32 | if (layr->up == NULL || layr->up->ctrlcmd == NULL) |
33 | return; |
34 | |
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: |
86 | pr_warn("Unexpected ctrl in cfsrvl (%d)\n" , ctrl); |
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); |
97 | |
98 | caif_assert(layr != NULL); |
99 | caif_assert(layr->dn != NULL); |
100 | caif_assert(layr->dn->transmit != NULL); |
101 | |
102 | if (!service->supports_flowctrl) |
103 | return 0; |
104 | |
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); |
112 | if (!pkt) |
113 | return -ENOMEM; |
114 | |
115 | if (cfpkt_add_head(pkt, data: &flow_on, len: 1) < 0) { |
116 | pr_err("Packet is erroneous!\n" ); |
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; |
124 | cfpkt_set_prio(pkt, TC_PRIO_CONTROL); |
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); |
133 | if (!pkt) |
134 | return -ENOMEM; |
135 | |
136 | if (cfpkt_add_head(pkt, data: &flow_off, len: 1) < 0) { |
137 | pr_err("Packet is erroneous!\n" ); |
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; |
145 | cfpkt_set_prio(pkt, TC_PRIO_CONTROL); |
146 | return layr->dn->transmit(layr->dn, pkt); |
147 | } |
148 | default: |
149 | break; |
150 | } |
151 | return -EINVAL; |
152 | } |
153 | |
154 | static void cfsrvl_release(struct cflayer *layer) |
155 | { |
156 | struct cfsrvl *service = container_of(layer, struct cfsrvl, layer); |
157 | kfree(objp: service); |
158 | } |
159 | |
160 | void cfsrvl_init(struct cfsrvl *service, |
161 | u8 channel_id, |
162 | struct dev_info *dev_info, |
163 | bool supports_flowctrl) |
164 | { |
165 | caif_assert(offsetof(struct cfsrvl, layer) == 0); |
166 | service->open = false; |
167 | service->modem_flow_on = true; |
168 | service->phy_flow_on = true; |
169 | service->layer.id = channel_id; |
170 | service->layer.ctrlcmd = cfservl_ctrlcmd; |
171 | service->layer.modemcmd = cfservl_modemcmd; |
172 | service->dev_info = *dev_info; |
173 | service->supports_flowctrl = supports_flowctrl; |
174 | service->release = cfsrvl_release; |
175 | } |
176 | |
177 | bool cfsrvl_ready(struct cfsrvl *service, int *err) |
178 | { |
179 | if (!service->open) { |
180 | *err = -ENOTCONN; |
181 | return false; |
182 | } |
183 | return true; |
184 | } |
185 | |
186 | u8 cfsrvl_getphyid(struct cflayer *layer) |
187 | { |
188 | struct cfsrvl *servl = container_obj(layer); |
189 | return servl->dev_info.id; |
190 | } |
191 | |
192 | bool cfsrvl_phyid_match(struct cflayer *layer, int phyid) |
193 | { |
194 | struct cfsrvl *servl = container_obj(layer); |
195 | return servl->dev_info.id == phyid; |
196 | } |
197 | |
198 | void caif_free_client(struct cflayer *adap_layer) |
199 | { |
200 | struct cfsrvl *servl; |
201 | if (adap_layer == NULL || adap_layer->dn == NULL) |
202 | return; |
203 | servl = container_obj(adap_layer->dn); |
204 | servl->release(&servl->layer); |
205 | } |
206 | EXPORT_SYMBOL(caif_free_client); |
207 | |
208 | void caif_client_register_refcnt(struct cflayer *adapt_layer, |
209 | void (*hold)(struct cflayer *lyr), |
210 | void (*put)(struct cflayer *lyr)) |
211 | { |
212 | struct cfsrvl *service; |
213 | |
214 | if (WARN_ON(adapt_layer == NULL || adapt_layer->dn == NULL)) |
215 | return; |
216 | service = container_of(adapt_layer->dn, struct cfsrvl, layer); |
217 | service->hold = hold; |
218 | service->put = put; |
219 | } |
220 | EXPORT_SYMBOL(caif_client_register_refcnt); |
221 | |