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/kernel.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) #include <linux/types.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/errno.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) #include <net/caif/caif_layer.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14) #include <net/caif/cfsrvl.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) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) #define container_obj(layr) ((struct cfsrvl *) layr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22) struct cflayer *cfvidl_create(u8 channel_id, struct dev_info *dev_info)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) 	struct cfsrvl *vid = kzalloc(sizeof(struct cfsrvl), GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) 	if (!vid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) 		return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) 	caif_assert(offsetof(struct cfsrvl, layer) == 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) 	cfsrvl_init(vid, channel_id, dev_info, false);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) 	vid->layer.receive = cfvidl_receive;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) 	vid->layer.transmit = cfvidl_transmit;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32) 	snprintf(vid->layer.name, CAIF_LAYER_NAME_SZ, "vid1");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) 	return &vid->layer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) static int cfvidl_receive(struct cflayer *layr, struct cfpkt *pkt)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) 	u32 videoheader;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) 	if (cfpkt_extr_head(pkt, &videoheader, 4) < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) 		pr_err("Packet is erroneous!\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) 		return -EPROTO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) 	return layr->up->receive(layr->up, pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) static int cfvidl_transmit(struct cflayer *layr, struct cfpkt *pkt)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) 	struct cfsrvl *service = container_obj(layr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) 	struct caif_payload_info *info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) 	u32 videoheader = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) 	int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) 	if (!cfsrvl_ready(service, &ret)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) 		cfpkt_destroy(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) 		return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) 	}
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) 
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) 	cfpkt_add_head(pkt, &videoheader, 4);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) 	/* Add info for MUX-layer to route the packet out */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) 	info = cfpkt_info(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62) 	info->channel_id = service->layer.id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) 	info->dev_info = &service->dev_info;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) 	return layr->dn->transmit(layr->dn, pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65) }