Orange Pi5 kernel

^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/stddef.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  10) #include <linux/slab.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  11) #include <net/caif/caif_layer.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  12) #include <net/caif/cfsrvl.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  13) #include <net/caif/cfpkt.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  14) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  15) #define VEI_PAYLOAD  0x00
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  16) #define VEI_CMD_BIT  0x80
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  17) #define VEI_FLOW_OFF 0x81
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  18) #define VEI_FLOW_ON  0x80
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  19) #define VEI_SET_PIN  0x82
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  20) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  21) #define container_obj(layr) container_of(layr, struct cfsrvl, layer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  22) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  23) static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  24) static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  25) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  26) struct cflayer *cfvei_create(u8 channel_id, struct dev_info *dev_info)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  27) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  28) 	struct cfsrvl *vei = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  29) 	if (!vei)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  30) 		return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  31) 	caif_assert(offsetof(struct cfsrvl, layer) == 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  32) 	cfsrvl_init(vei, channel_id, dev_info, true);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  33) 	vei->layer.receive = cfvei_receive;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  34) 	vei->layer.transmit = cfvei_transmit;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  35) 	snprintf(vei->layer.name, CAIF_LAYER_NAME_SZ, "vei%d", channel_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  36) 	return &vei->layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  37) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  38) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  39) static int cfvei_receive(struct cflayer *layr, struct cfpkt *pkt)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  40) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  41) 	u8 cmd;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  42) 	int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  43) 	caif_assert(layr->up != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  44) 	caif_assert(layr->receive != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  45) 	caif_assert(layr->ctrlcmd != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  46) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  47) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  48) 	if (cfpkt_extr_head(pkt, &cmd, 1) < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  49) 		pr_err("Packet is erroneous!\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  50) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  51) 		return -EPROTO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  52) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  53) 	switch (cmd) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  54) 	case VEI_PAYLOAD:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  55) 		ret = layr->up->receive(layr->up, pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  56) 		return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  57) 	case VEI_FLOW_OFF:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  58) 		layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_OFF_IND, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  59) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  60) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  61) 	case VEI_FLOW_ON:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  62) 		layr->ctrlcmd(layr, CAIF_CTRLCMD_FLOW_ON_IND, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  63) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  64) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  65) 	case VEI_SET_PIN:	/* SET RS232 PIN */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  66) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  67) 		return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  68) 	default:		/* SET RS232 PIN */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  69) 		pr_warn("Unknown VEI control packet %d (0x%x)!\n", cmd, cmd);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  70) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  71) 		return -EPROTO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  72) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  73) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  74) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  75) static int cfvei_transmit(struct cflayer *layr, struct cfpkt *pkt)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  76) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  77) 	u8 tmp = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  78) 	struct caif_payload_info *info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  79) 	int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  80) 	struct cfsrvl *service = container_obj(layr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  81) 	if (!cfsrvl_ready(service, &ret))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  82) 		goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  83) 	caif_assert(layr->dn != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  84) 	caif_assert(layr->dn->transmit != NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  85) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  86) 	if (cfpkt_add_head(pkt, &tmp, 1) < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  87) 		pr_err("Packet is erroneous!\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  88) 		ret = -EPROTO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  89) 		goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  90) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  91) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  92) 	/* Add info-> for MUX-layer to route the packet out. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  93) 	info = cfpkt_info(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  94) 	info->channel_id = service->layer.id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  95) 	info->hdr_len = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  96) 	info->dev_info = &service->dev_info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  97) 	return layr->dn->transmit(layr->dn, pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  98) err:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300  99) 	cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) 	return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) }