^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0-only
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) * Copyright (C) ST-Ericsson AB 2010
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) * Author: Sjur Brendeland
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7) #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) #include <linux/kernel.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) #include <linux/stddef.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) #include <linux/slab.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12) #include <linux/netdevice.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) #include <linux/module.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14) #include <net/caif/caif_layer.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) #include <net/caif/cfpkt.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) #include <net/caif/cfcnfg.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) #include <net/caif/cfctrl.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) #include <net/caif/cfmuxl.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) #include <net/caif/cffrml.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) #include <net/caif/cfserl.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) #include <net/caif/cfsrvl.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22) #include <net/caif/caif_dev.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) #define container_obj(layr) container_of(layr, struct cfcnfg, layer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) /* Information about CAIF physical interfaces held by Config Module in order
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) * to manage physical interfaces
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) struct cfcnfg_phyinfo {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) struct list_head node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) bool up;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) /* Pointer to the layer below the MUX (framing layer) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) struct cflayer *frm_layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) /* Pointer to the lowest actual physical layer */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) struct cflayer *phy_layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) /* Unique identifier of the physical interface */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) unsigned int id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) /* Preference of the physical in interface */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) enum cfcnfg_phy_preference pref;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) /* Information about the physical device */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) struct dev_info dev_info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) /* Interface index */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) int ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) /* Protocol head room added for CAIF link layer */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) int head_room;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) /* Use Start of frame checksum */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) bool use_fcs;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) struct cfcnfg {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) struct cflayer layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) struct cflayer *ctrl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) struct cflayer *mux;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) struct list_head phys;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) struct mutex lock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) static void cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) enum cfctrl_srv serv, u8 phyid,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65) struct cflayer *adapt_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66) static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68) struct cflayer *adapt_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69) static void cfctrl_resp_func(void);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70) static void cfctrl_enum_resp(void);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72) struct cfcnfg *cfcnfg_create(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74) struct cfcnfg *this;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) struct cfctrl_rsp *resp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77) might_sleep();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) /* Initiate this layer */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) this = kzalloc(sizeof(struct cfcnfg), GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81) if (!this)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83) this->mux = cfmuxl_create();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84) if (!this->mux)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85) goto out_of_mem;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) this->ctrl = cfctrl_create();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87) if (!this->ctrl)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) goto out_of_mem;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) /* Initiate response functions */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90) resp = cfctrl_get_respfuncs(this->ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91) resp->enum_rsp = cfctrl_enum_resp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92) resp->linkerror_ind = cfctrl_resp_func;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93) resp->linkdestroy_rsp = cfcnfg_linkdestroy_rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94) resp->sleep_rsp = cfctrl_resp_func;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95) resp->wake_rsp = cfctrl_resp_func;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96) resp->restart_rsp = cfctrl_resp_func;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97) resp->radioset_rsp = cfctrl_resp_func;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) resp->linksetup_rsp = cfcnfg_linkup_rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99) resp->reject_rsp = cfcnfg_reject_rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) INIT_LIST_HEAD(&this->phys);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) cfmuxl_set_uplayer(this->mux, this->ctrl, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) layer_set_dn(this->ctrl, this->mux);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) layer_set_up(this->ctrl, this);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) mutex_init(&this->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) return this;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) out_of_mem:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) synchronize_rcu();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) kfree(this->mux);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) kfree(this->ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) kfree(this);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) void cfcnfg_remove(struct cfcnfg *cfg)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) might_sleep();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) if (cfg) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) synchronize_rcu();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) kfree(cfg->mux);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) cfctrl_remove(cfg->ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) kfree(cfg);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) static void cfctrl_resp_func(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) static struct cfcnfg_phyinfo *cfcnfg_get_phyinfo_rcu(struct cfcnfg *cnfg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) u8 phyid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) struct cfcnfg_phyinfo *phy;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) list_for_each_entry_rcu(phy, &cnfg->phys, node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) if (phy->id == phyid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) return phy;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) static void cfctrl_enum_resp(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) static struct dev_info *cfcnfg_get_phyid(struct cfcnfg *cnfg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) enum cfcnfg_phy_preference phy_pref)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) /* Try to match with specified preference */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) struct cfcnfg_phyinfo *phy;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) list_for_each_entry_rcu(phy, &cnfg->phys, node) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) if (phy->up && phy->pref == phy_pref &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) phy->frm_layer != NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) return &phy->dev_info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) /* Otherwise just return something */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) list_for_each_entry_rcu(phy, &cnfg->phys, node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) if (phy->up)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) return &phy->dev_info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) static int cfcnfg_get_id_from_ifi(struct cfcnfg *cnfg, int ifi)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) struct cfcnfg_phyinfo *phy;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) list_for_each_entry_rcu(phy, &cnfg->phys, node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) if (phy->ifindex == ifi && phy->up)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) return phy->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) return -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) int caif_disconnect_client(struct net *net, struct cflayer *adap_layer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) u8 channel_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) struct cfcnfg *cfg = get_cfcnfg(net);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) caif_assert(adap_layer != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) cfctrl_cancel_req(cfg->ctrl, adap_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) channel_id = adap_layer->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) if (channel_id != 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) struct cflayer *servl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) servl = cfmuxl_remove_uplayer(cfg->mux, channel_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) cfctrl_linkdown_req(cfg->ctrl, channel_id, adap_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) if (servl != NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) layer_set_up(servl, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) } else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) pr_debug("nothing to disconnect\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) /* Do RCU sync before initiating cleanup */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) synchronize_rcu();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) if (adap_layer->ctrlcmd != NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) adap_layer->ctrlcmd(adap_layer, CAIF_CTRLCMD_DEINIT_RSP, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) EXPORT_SYMBOL(caif_disconnect_client);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) static void cfcnfg_linkdestroy_rsp(struct cflayer *layer, u8 channel_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) static const int protohead[CFCTRL_SRV_MASK] = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) [CFCTRL_SRV_VEI] = 4,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) [CFCTRL_SRV_DATAGRAM] = 7,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) [CFCTRL_SRV_UTIL] = 4,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) [CFCTRL_SRV_RFM] = 3,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) [CFCTRL_SRV_DBG] = 3,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) static int caif_connect_req_to_link_param(struct cfcnfg *cnfg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) struct caif_connect_request *s,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) struct cfctrl_link_param *l)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) struct dev_info *dev_info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) enum cfcnfg_phy_preference pref;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) int res;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) memset(l, 0, sizeof(*l));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) /* In caif protocol low value is high priority */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) l->priority = CAIF_PRIO_MAX - s->priority + 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) if (s->ifindex != 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) res = cfcnfg_get_id_from_ifi(cnfg, s->ifindex);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) if (res < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) return res;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) l->phyid = res;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) switch (s->link_selector) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) case CAIF_LINK_HIGH_BANDW:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) pref = CFPHYPREF_HIGH_BW;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240) case CAIF_LINK_LOW_LATENCY:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) pref = CFPHYPREF_LOW_LAT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) dev_info = cfcnfg_get_phyid(cnfg, pref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) if (dev_info == NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) return -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249) l->phyid = dev_info->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) switch (s->protocol) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) case CAIFPROTO_AT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253) l->linktype = CFCTRL_SRV_VEI;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) l->endpoint = (s->sockaddr.u.at.type >> 2) & 0x3;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) l->chtype = s->sockaddr.u.at.type & 0x3;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257) case CAIFPROTO_DATAGRAM:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258) l->linktype = CFCTRL_SRV_DATAGRAM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) l->chtype = 0x00;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260) l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262) case CAIFPROTO_DATAGRAM_LOOP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) l->linktype = CFCTRL_SRV_DATAGRAM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) l->chtype = 0x03;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) l->endpoint = 0x00;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) l->u.datagram.connid = s->sockaddr.u.dgm.connection_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) case CAIFPROTO_RFM:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) l->linktype = CFCTRL_SRV_RFM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) l->u.datagram.connid = s->sockaddr.u.rfm.connection_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) strlcpy(l->u.rfm.volume, s->sockaddr.u.rfm.volume,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) sizeof(l->u.rfm.volume));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) case CAIFPROTO_UTIL:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) l->linktype = CFCTRL_SRV_UTIL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) l->endpoint = 0x00;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) l->chtype = 0x00;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278) strlcpy(l->u.utility.name, s->sockaddr.u.util.service,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) sizeof(l->u.utility.name));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280) caif_assert(sizeof(l->u.utility.name) > 10);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) l->u.utility.paramlen = s->param.size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282) if (l->u.utility.paramlen > sizeof(l->u.utility.params))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) l->u.utility.paramlen = sizeof(l->u.utility.params);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) memcpy(l->u.utility.params, s->param.data,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) l->u.utility.paramlen);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) case CAIFPROTO_DEBUG:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290) l->linktype = CFCTRL_SRV_DBG;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) l->endpoint = s->sockaddr.u.dbg.service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) l->chtype = s->sockaddr.u.dbg.type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) int caif_connect_client(struct net *net, struct caif_connect_request *conn_req,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301) struct cflayer *adap_layer, int *ifindex,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) int *proto_head, int *proto_tail)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304) struct cflayer *frml;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) struct cfcnfg_phyinfo *phy;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) struct cfctrl_link_param param;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) struct cfcnfg *cfg = get_cfcnfg(net);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) err = caif_connect_req_to_link_param(cfg, conn_req, ¶m);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315) phy = cfcnfg_get_phyinfo_rcu(cfg, param.phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) if (!phy) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317) err = -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320) err = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) if (adap_layer == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323) pr_err("adap_layer is zero\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) if (adap_layer->receive == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) pr_err("adap_layer->receive is NULL\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) if (adap_layer->ctrlcmd == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) pr_err("adap_layer->ctrlcmd == NULL\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) err = -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336) frml = phy->frm_layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) if (frml == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) pr_err("Specified PHY type does not exist!\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) caif_assert(param.phyid == phy->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) caif_assert(phy->frm_layer->id ==
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343) param.phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) caif_assert(phy->phy_layer->id ==
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) param.phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) *ifindex = phy->ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348) *proto_tail = 2;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349) *proto_head = protohead[param.linktype] + phy->head_room;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) /* FIXME: ENUMERATE INITIALLY WHEN ACTIVATING PHYSICAL INTERFACE */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354) cfctrl_enum_req(cfg->ctrl, param.phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355) return cfctrl_linkup_request(cfg->ctrl, ¶m, adap_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) unlock:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) return err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361) EXPORT_SYMBOL(caif_connect_client);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) static void cfcnfg_reject_rsp(struct cflayer *layer, u8 channel_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) struct cflayer *adapt_layer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) adapt_layer->ctrlcmd(adapt_layer,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) static void
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372) cfcnfg_linkup_rsp(struct cflayer *layer, u8 channel_id, enum cfctrl_srv serv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) u8 phyid, struct cflayer *adapt_layer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375) struct cfcnfg *cnfg = container_obj(layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376) struct cflayer *servicel = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377) struct cfcnfg_phyinfo *phyinfo;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378) struct net_device *netdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380) if (channel_id == 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381) pr_warn("received channel_id zero\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382) if (adapt_layer != NULL && adapt_layer->ctrlcmd != NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) adapt_layer->ctrlcmd(adapt_layer,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384) CAIF_CTRLCMD_INIT_FAIL_RSP, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) if (adapt_layer == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391) pr_debug("link setup response but no client exist, send linkdown back\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) cfctrl_linkdown_req(cnfg->ctrl, channel_id, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) caif_assert(cnfg != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397) caif_assert(phyid != 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400) if (phyinfo == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401) pr_err("ERROR: Link Layer Device disappeared while connecting\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405) caif_assert(phyinfo != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406) caif_assert(phyinfo->id == phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) caif_assert(phyinfo->phy_layer != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) caif_assert(phyinfo->phy_layer->id == phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410) adapt_layer->id = channel_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412) switch (serv) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) case CFCTRL_SRV_VEI:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414) servicel = cfvei_create(channel_id, &phyinfo->dev_info);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) case CFCTRL_SRV_DATAGRAM:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417) servicel = cfdgml_create(channel_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418) &phyinfo->dev_info);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420) case CFCTRL_SRV_RFM:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 421) netdev = phyinfo->dev_info.dev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 422) servicel = cfrfml_create(channel_id, &phyinfo->dev_info,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 423) netdev->mtu);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 424) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 425) case CFCTRL_SRV_UTIL:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 426) servicel = cfutill_create(channel_id, &phyinfo->dev_info);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 427) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 428) case CFCTRL_SRV_VIDEO:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 429) servicel = cfvidl_create(channel_id, &phyinfo->dev_info);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 430) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 431) case CFCTRL_SRV_DBG:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 432) servicel = cfdbgl_create(channel_id, &phyinfo->dev_info);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 433) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 434) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 435) pr_err("Protocol error. Link setup response - unknown channel type\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 436) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 437) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 438) if (!servicel)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 439) goto unlock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 440) layer_set_dn(servicel, cnfg->mux);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 441) cfmuxl_set_uplayer(cnfg->mux, servicel, channel_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 442) layer_set_up(servicel, adapt_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 443) layer_set_dn(adapt_layer, servicel);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 444)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 445) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 446)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 447) servicel->ctrlcmd(servicel, CAIF_CTRLCMD_INIT_RSP, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 448) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 449) unlock:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 450) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 451) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 452)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 453) int
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 454) cfcnfg_add_phy_layer(struct cfcnfg *cnfg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 455) struct net_device *dev, struct cflayer *phy_layer,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 456) enum cfcnfg_phy_preference pref,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 457) struct cflayer *link_support,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 458) bool fcs, int head_room)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 459) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 460) struct cflayer *frml;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 461) struct cfcnfg_phyinfo *phyinfo = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 462) int i, res = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 463) u8 phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 464)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 465) mutex_lock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 466)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 467) /* CAIF protocol allow maximum 6 link-layers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 468) for (i = 0; i < 7; i++) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 469) phyid = (dev->ifindex + i) & 0x7;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 470) if (phyid == 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 471) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 472) if (cfcnfg_get_phyinfo_rcu(cnfg, phyid) == NULL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 473) goto got_phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 474) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 475) pr_warn("Too many CAIF Link Layers (max 6)\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 476) res = -EEXIST;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 477) goto out;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 478)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 479) got_phyid:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 480) phyinfo = kzalloc(sizeof(struct cfcnfg_phyinfo), GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 481) if (!phyinfo) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 482) res = -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 483) goto out_err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 484) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 485)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 486) phy_layer->id = phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 487) phyinfo->pref = pref;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 488) phyinfo->id = phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 489) phyinfo->dev_info.id = phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 490) phyinfo->dev_info.dev = dev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 491) phyinfo->phy_layer = phy_layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 492) phyinfo->ifindex = dev->ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 493) phyinfo->head_room = head_room;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 494) phyinfo->use_fcs = fcs;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 495)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 496) frml = cffrml_create(phyid, fcs);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 497)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 498) if (!frml) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 499) res = -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 500) goto out_err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 501) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 502) phyinfo->frm_layer = frml;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 503) layer_set_up(frml, cnfg->mux);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 504)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 505) if (link_support != NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 506) link_support->id = phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 507) layer_set_dn(frml, link_support);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 508) layer_set_up(link_support, frml);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 509) layer_set_dn(link_support, phy_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 510) layer_set_up(phy_layer, link_support);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 511) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 512) layer_set_dn(frml, phy_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 513) layer_set_up(phy_layer, frml);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 514) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 515)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 516) list_add_rcu(&phyinfo->node, &cnfg->phys);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 517) out:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 518) mutex_unlock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 519) return res;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 520)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 521) out_err:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 522) kfree(phyinfo);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 523) mutex_unlock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 524) return res;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 525) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 526) EXPORT_SYMBOL(cfcnfg_add_phy_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 527)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 528) int cfcnfg_set_phy_state(struct cfcnfg *cnfg, struct cflayer *phy_layer,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 529) bool up)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 530) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 531) struct cfcnfg_phyinfo *phyinfo;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 532)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 533) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 534) phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phy_layer->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 535) if (phyinfo == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 536) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 537) return -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 538) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 539)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 540) if (phyinfo->up == up) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 541) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 542) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 543) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 544) phyinfo->up = up;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 545)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 546) if (up) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 547) cffrml_hold(phyinfo->frm_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 548) cfmuxl_set_dnlayer(cnfg->mux, phyinfo->frm_layer,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 549) phy_layer->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 550) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 551) cfmuxl_remove_dnlayer(cnfg->mux, phy_layer->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 552) cffrml_put(phyinfo->frm_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 553) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 554)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 555) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 556) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 557) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 558) EXPORT_SYMBOL(cfcnfg_set_phy_state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 559)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 560) int cfcnfg_del_phy_layer(struct cfcnfg *cnfg, struct cflayer *phy_layer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 561) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 562) struct cflayer *frml, *frml_dn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 563) u16 phyid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 564) struct cfcnfg_phyinfo *phyinfo;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 565)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 566) might_sleep();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 567)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 568) mutex_lock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 569)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 570) phyid = phy_layer->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 571) phyinfo = cfcnfg_get_phyinfo_rcu(cnfg, phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 572)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 573) if (phyinfo == NULL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 574) mutex_unlock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 575) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 576) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 577) caif_assert(phyid == phyinfo->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 578) caif_assert(phy_layer == phyinfo->phy_layer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 579) caif_assert(phy_layer->id == phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 580) caif_assert(phyinfo->frm_layer->id == phyid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 581)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 582) list_del_rcu(&phyinfo->node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 583) synchronize_rcu();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 584)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 585) /* Fail if reference count is not zero */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 586) if (cffrml_refcnt_read(phyinfo->frm_layer) != 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 587) pr_info("Wait for device inuse\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 588) list_add_rcu(&phyinfo->node, &cnfg->phys);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 589) mutex_unlock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 590) return -EAGAIN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 591) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 592)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 593) frml = phyinfo->frm_layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 594) frml_dn = frml->dn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 595) cffrml_set_uplayer(frml, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 596) cffrml_set_dnlayer(frml, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 597) if (phy_layer != frml_dn) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 598) layer_set_up(frml_dn, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 599) layer_set_dn(frml_dn, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 600) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 601) layer_set_up(phy_layer, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 602)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 603) if (phyinfo->phy_layer != frml_dn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 604) kfree(frml_dn);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 605)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 606) cffrml_free(frml);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 607) kfree(phyinfo);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 608) mutex_unlock(&cnfg->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 609)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 610) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 611) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 612) EXPORT_SYMBOL(cfcnfg_del_phy_layer);
Orange Pi5 kernel
Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards
3 Commits
0 Branches
0 Tags