net/caif/cfveil.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Copyright (C) ST-Ericsson AB 2010
0004  * Author:  Sjur Brendeland
0005  */
0006
0007 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
0008
0009 #include <linux/stddef.h>
0010 #include <linux/slab.h>
0011 #include <net/caif/caif_layer.h>
0012 #include <net/caif/cfsrvl.h>
0013 #include <net/caif/cfpkt.h>
0014
0015 #define VEI_PAYLOAD  0x00
0016 #define VEI_CMD_BIT  0x80
0017 #define VEI_FLOW_OFF 0x81
0018 #define VEI_FLOW_ON  0x80
0019 #define VEI_SET_PIN  0x82
0020
0021 #define container_obj(layr) container_of(layr, struct cfsrvl, layer)
0022
0023 static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt);
0024 static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt);
0025
0026 struct cflayer *cfvei_create(u8 channel_id, struct dev_info *dev_info)
0027 {
0028     struct cfsrvl *vei = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
0029     if (!vei)
0030         return NULL;
0031     caif_assert(offsetof(struct cfsrvl, layer) == 0);
0032     cfsrvl_init(vei, channel_id, dev_info, true);
0033     vei->layer.receive = cfvei_receive;
0034     vei->layer.transmit = cfvei_transmit;
0035     snprintf(vei->layer.name, CAIF_LAYER_NAME_SZ, "vei%d", channel_id);
0036     return &vei->layer;
0037 }
0038
0039 static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt)
0040 {
0041     u8 cmd;
0042     int ret;
0043     caif_assert(layr->up != NULL);
0044     caif_assert(layr->receive != NULL);
0045     caif_assert(layr->ctrlcmd != NULL);
0046
0047
0048     if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
0049         pr_err("Packet is erroneous!\n");
0050         cfpkt_destroy(pkt);
0051         return -EPROTO;
0052     }
0053     switch (cmd) {
0054     case VEI_PAYLOAD:
0055         ret = layr->up->receive(layr->up, pkt);
0056         return ret;
0057     case VEI_FLOW_OFF:
0058         layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
0059         cfpkt_destroy(pkt);
0060         return 0;
0061     case VEI_FLOW_ON:
0062         layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
0063         cfpkt_destroy(pkt);
0064         return 0;
0065     case VEI_SET_PIN:   /* SET RS232 PIN */
0066         cfpkt_destroy(pkt);
0067         return 0;
0068     default:        /* SET RS232 PIN */
0069         pr_warn("Unknown VEI control packet %d (0x%x)!\n", cmd, cmd);
0070         cfpkt_destroy(pkt);
0071         return -EPROTO;
0072     }
0073 }
0074
0075 static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt)
0076 {
0077     u8 tmp = 0;
0078     struct caif_payload_info *info;
0079     int ret;
0080     struct cfsrvl *service = container_obj(layr);
0081     if (!cfsrvl_ready(service, &ret))
0082         goto err;
0083     caif_assert(layr->dn != NULL);
0084     caif_assert(layr->dn->transmit != NULL);
0085
0086     if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
0087         pr_err("Packet is erroneous!\n");
0088         ret = -EPROTO;
0089         goto err;
0090     }
0091
0092     /* Add info-> for MUX-layer to route the packet out. */
0093     info = cfpkt_info(pkt);
0094     info->channel_id = service->layer.id;
0095     info->hdr_len = 1;
0096     info->dev_info = &service->dev_info;
0097     return layr->dn->transmit(layr->dn, pkt);
0098 err:
0099     cfpkt_destroy(pkt);
0100     return ret;
0101 }