Merge "net: ipc_router: Add Optional header in IPC Router V2" (186138d8) · Commits · e / devices / android_kernel_xiaomi_markw

include/linux/ipc_router_xprt.h

+17 −2

Original line number	Original line	Diff line number	Diff line
	/* Copyright (c) 2011-2014, The Linux Foundation. All rights reserved.		/* Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
	*		*
	* This program is free software; you can redistribute it and/or modify		* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License version 2 and		* it under the terms of the GNU General Public License version 2 and
	@@ -54,6 +54,7 @@ struct rr_header_v1 {
	* @version: Version information.		* @version: Version information.
	* @type: IPC Router Message Type.		* @type: IPC Router Message Type.
	* @control_flag: Flags to indicate flow control, optional header etc.		* @control_flag: Flags to indicate flow control, optional header etc.
			* @opt_len: Combined size of the all optional headers in units of words.
	* @size: Size of the IPC Router payload.		* @size: Size of the IPC Router payload.
	* @src_node_id: Source Node ID of the message.		* @src_node_id: Source Node ID of the message.
	* @src_port_id: Source Port ID of the message.		* @src_port_id: Source Port ID of the message.
	@@ -63,7 +64,8 @@ struct rr_header_v1 {
	struct rr_header_v2 {		struct rr_header_v2 {
	uint8_t version;		uint8_t version;
	uint8_t type;		uint8_t type;
	uint16_t control_flag;		uint8_t control_flag;
			uint8_t opt_len;
	uint32_t size;		uint32_t size;
	uint16_t src_node_id;		uint16_t src_node_id;
	uint16_t src_port_id;		uint16_t src_port_id;
	@@ -76,18 +78,31 @@ union rr_header {
	struct rr_header_v2 hdr_v2;		struct rr_header_v2 hdr_v2;
	};		};

			/**
			* rr_opt_hdr - Optional header for IPC Router header version 2
			* @len: Total length of the optional header.
			* @data: Pointer to the actual optional header.
			*/
			struct rr_opt_hdr {
			size_t len;
			unsigned char *data;
			};

	#define IPC_ROUTER_HDR_SIZE sizeof(union rr_header)		#define IPC_ROUTER_HDR_SIZE sizeof(union rr_header)
			#define IPCR_WORD_SIZE 4

	/**		/**
	* rr_packet - Router to Router packet structure		* rr_packet - Router to Router packet structure
	* @list: Pointer to prev & next packets in a port's rx list.		* @list: Pointer to prev & next packets in a port's rx list.
	* @hdr: Header information extracted from or prepended to a packet.		* @hdr: Header information extracted from or prepended to a packet.
			* @opt_hdr: Optinal header information.
	* @pkt_fragment_q: Queue of SKBs containing payload.		* @pkt_fragment_q: Queue of SKBs containing payload.
	* @length: Length of data in the chain of SKBs		* @length: Length of data in the chain of SKBs
	*/		*/
	struct rr_packet {		struct rr_packet {
	struct list_head list;		struct list_head list;
	struct rr_header_v1 hdr;		struct rr_header_v1 hdr;
			struct rr_opt_hdr opt_hdr;
	struct sk_buff_head *pkt_fragment_q;		struct sk_buff_head *pkt_fragment_q;
	uint32_t length;		uint32_t length;
	};		};

net/ipc_router/ipc_router_core.c

+71 −12

Original line number	Original line	Diff line number	Diff line
	@@ -500,7 +500,17 @@ struct rr_packet clone_pkt(struct rr_packet pkt)
	return NULL;		return NULL;
	}		}
	memcpy(&(cloned_pkt->hdr), &(pkt->hdr), sizeof(struct rr_header_v1));		memcpy(&(cloned_pkt->hdr), &(pkt->hdr), sizeof(struct rr_header_v1));
	/* TODO: Copy optional headers, if available */		if (pkt->opt_hdr.len > 0) {
			cloned_pkt->opt_hdr.data = kmalloc(pkt->opt_hdr.len,
			GFP_KERNEL);
			if (!cloned_pkt->opt_hdr.data) {
			IPC_RTR_ERR("%s: Memory allocation Failed\n", __func__);
			} else {
			cloned_pkt->opt_hdr.len = pkt->opt_hdr.len;
			memcpy(cloned_pkt->opt_hdr.data, pkt->opt_hdr.data,
			pkt->opt_hdr.len);
			}
			}

	pkt_fragment_q = kmalloc(sizeof(struct sk_buff_head), GFP_KERNEL);		pkt_fragment_q = kmalloc(sizeof(struct sk_buff_head), GFP_KERNEL);
	if (!pkt_fragment_q) {		if (!pkt_fragment_q) {
	@@ -526,7 +536,8 @@ fail_clone:
	kfree_skb(temp_skb);		kfree_skb(temp_skb);
	}		}
	kfree(pkt_fragment_q);		kfree(pkt_fragment_q);
	/* TODO: Free optional headers, if present */		if (cloned_pkt->opt_hdr.len > 0)
			kfree(cloned_pkt->opt_hdr.data);
	kfree(cloned_pkt);		kfree(cloned_pkt);
	return NULL;		return NULL;
	}		}
	@@ -583,7 +594,8 @@ void release_pkt(struct rr_packet *pkt)
	kfree_skb(temp_skb);		kfree_skb(temp_skb);
	}		}
	kfree(pkt->pkt_fragment_q);		kfree(pkt->pkt_fragment_q);
	/* TODO: Free Optional headers, if present */		if (pkt->opt_hdr.len > 0)
			kfree(pkt->opt_hdr.data);
	kfree(pkt);		kfree(pkt);
	return;		return;
	}		}
	@@ -692,6 +704,42 @@ void msm_ipc_router_free_skb(struct sk_buff_head *skb_head)
	kfree(skb_head);		kfree(skb_head);
	}		}

			/**
			* extract_optional_header() - Extract the optional header from skb
			* @pkt: Packet structure into which the header has to be extracted.
			* @opt_len: The optional header length in word size.
			*
			* @return: Length of optional header in bytes if success, zero otherwise.
			*/
			static int extract_optional_header(struct rr_packet *pkt, uint8_t opt_len)
			{
			size_t offset = 0, buf_len = 0, copy_len, opt_hdr_len;
			struct sk_buff *temp;
			struct sk_buff_head *skb_head;

			opt_hdr_len = opt_len * IPCR_WORD_SIZE;
			pkt->opt_hdr.data = kmalloc(opt_hdr_len, GFP_KERNEL);
			if (!pkt->opt_hdr.data) {
			IPC_RTR_ERR("%s: Memory allocation Failed\n", __func__);
			return 0;
			}
			skb_head = pkt->pkt_fragment_q;
			buf_len = opt_hdr_len;
			skb_queue_walk(skb_head, temp) {
			copy_len = buf_len < temp->len ? buf_len : temp->len;
			memcpy(pkt->opt_hdr.data + offset, temp->data, copy_len);
			offset += copy_len;
			buf_len -= copy_len;
			skb_pull(temp, copy_len);
			if (temp->len == 0) {
			skb_dequeue(skb_head);
			kfree_skb(temp);
			}
			}
			pkt->opt_hdr.len = opt_hdr_len;
			return opt_hdr_len;
			}

	/**		/**
	* extract_header_v1() - Extract IPC Router header of version 1		* extract_header_v1() - Extract IPC Router header of version 1
	* @pkt: Packet structure into which the header has to be extraced.		* @pkt: Packet structure into which the header has to be extraced.
	@@ -722,6 +770,9 @@ static int extract_header_v1(struct rr_packet pkt, struct sk_buff skb)
	static int extract_header_v2(struct rr_packet pkt, struct sk_buff skb)		static int extract_header_v2(struct rr_packet pkt, struct sk_buff skb)
	{		{
	struct rr_header_v2 *hdr;		struct rr_header_v2 *hdr;
			uint8_t opt_len;
			size_t opt_hdr_len;
			size_t total_hdr_size = sizeof(*hdr);

	if (!pkt \|\| !skb) {		if (!pkt \|\| !skb) {
	IPC_RTR_ERR("%s: Invalid pkt or skb\n", __func__);		IPC_RTR_ERR("%s: Invalid pkt or skb\n", __func__);
	@@ -737,8 +788,13 @@ static int extract_header_v2(struct rr_packet pkt, struct sk_buff skb)
	pkt->hdr.control_flag = (uint32_t)hdr->control_flag;		pkt->hdr.control_flag = (uint32_t)hdr->control_flag;
	pkt->hdr.dst_node_id = (uint32_t)hdr->dst_node_id;		pkt->hdr.dst_node_id = (uint32_t)hdr->dst_node_id;
	pkt->hdr.dst_port_id = (uint32_t)hdr->dst_port_id;		pkt->hdr.dst_port_id = (uint32_t)hdr->dst_port_id;
	skb_pull(skb, sizeof(struct rr_header_v2));		opt_len = hdr->opt_len;
	pkt->length -= sizeof(struct rr_header_v2);		skb_pull(skb, total_hdr_size);
			if (opt_len > 0) {
			opt_hdr_len = extract_optional_header(pkt, opt_len);
			total_hdr_size += opt_hdr_len;
			}
			pkt->length -= total_hdr_size;
	return 0;		return 0;
	}		}

	@@ -771,7 +827,6 @@ static int extract_header(struct rr_packet *pkt)
	ret = extract_header_v1(pkt, temp_skb);		ret = extract_header_v1(pkt, temp_skb);
	} else if (temp_skb->data[0] == IPC_ROUTER_V2) {		} else if (temp_skb->data[0] == IPC_ROUTER_V2) {
	ret = extract_header_v2(pkt, temp_skb);		ret = extract_header_v2(pkt, temp_skb);
	/* TODO: Extract optional headers if present */
	} else {		} else {
	IPC_RTR_ERR("%s: Invalid Header version %02x\n",		IPC_RTR_ERR("%s: Invalid Header version %02x\n",
	__func__, temp_skb->data[0]);		__func__, temp_skb->data[0]);
	@@ -814,8 +869,7 @@ static int calc_tx_header_size(struct rr_packet *pkt,
	hdr_size = sizeof(struct rr_header_v1);		hdr_size = sizeof(struct rr_header_v1);
	} else if (xprt_version == IPC_ROUTER_V2) {		} else if (xprt_version == IPC_ROUTER_V2) {
	pkt->hdr.version = IPC_ROUTER_V2;		pkt->hdr.version = IPC_ROUTER_V2;
	hdr_size = sizeof(struct rr_header_v2);		hdr_size = sizeof(struct rr_header_v2) + pkt->opt_hdr.len;
	/* TODO: Calculate optional header length, if present */
	} else {		} else {
	IPC_RTR_ERR("%s: Invalid xprt_version %d\n",		IPC_RTR_ERR("%s: Invalid xprt_version %d\n",
	__func__, xprt_version);		__func__, xprt_version);
	@@ -923,13 +977,18 @@ static int prepend_header_v2(struct rr_packet *pkt, int hdr_size)
	hdr = (struct rr_header_v2 *)skb_push(temp_skb, hdr_size);		hdr = (struct rr_header_v2 *)skb_push(temp_skb, hdr_size);
	hdr->version = (uint8_t)pkt->hdr.version;		hdr->version = (uint8_t)pkt->hdr.version;
	hdr->type = (uint8_t)pkt->hdr.type;		hdr->type = (uint8_t)pkt->hdr.type;
	hdr->control_flag = (uint16_t)pkt->hdr.control_flag;		hdr->control_flag = (uint8_t)pkt->hdr.control_flag;
	hdr->size = (uint32_t)pkt->hdr.size;		hdr->size = (uint32_t)pkt->hdr.size;
	hdr->src_node_id = (uint16_t)pkt->hdr.src_node_id;		hdr->src_node_id = (uint16_t)pkt->hdr.src_node_id;
	hdr->src_port_id = (uint16_t)pkt->hdr.src_port_id;		hdr->src_port_id = (uint16_t)pkt->hdr.src_port_id;
	hdr->dst_node_id = (uint16_t)pkt->hdr.dst_node_id;		hdr->dst_node_id = (uint16_t)pkt->hdr.dst_node_id;
	hdr->dst_port_id = (uint16_t)pkt->hdr.dst_port_id;		hdr->dst_port_id = (uint16_t)pkt->hdr.dst_port_id;
	/* TODO: Add optional headers, if present */		if (pkt->opt_hdr.len > 0) {
			hdr->opt_len = pkt->opt_hdr.len/IPCR_WORD_SIZE;
			memcpy(hdr + sizeof(*hdr), pkt->opt_hdr.data, pkt->opt_hdr.len);
			} else {
			hdr->opt_len = 0;
			}
	if (temp_skb != skb_peek(pkt->pkt_fragment_q))		if (temp_skb != skb_peek(pkt->pkt_fragment_q))
	skb_queue_head(pkt->pkt_fragment_q, temp_skb);		skb_queue_head(pkt->pkt_fragment_q, temp_skb);
	pkt->length += hdr_size;		pkt->length += hdr_size;
	@@ -1088,13 +1147,13 @@ int ipc_router_peek_pkt_size(char *data)
	return -EINVAL;		return -EINVAL;
	}		}

	/* FUTURE: Calculate optional header len in V2 header*/
	if (data[0] == IPC_ROUTER_V1)		if (data[0] == IPC_ROUTER_V1)
	size = ((struct rr_header_v1 *)data)->size +		size = ((struct rr_header_v1 *)data)->size +
	sizeof(struct rr_header_v1);		sizeof(struct rr_header_v1);
	else if (data[0] == IPC_ROUTER_V2)		else if (data[0] == IPC_ROUTER_V2)
	size = ((struct rr_header_v2 *)data)->size +		size = ((struct rr_header_v2 *)data)->size +
	sizeof(struct rr_header_v2);		((struct rr_header_v2 )data)->opt_len IPCR_WORD_SIZE
			+ sizeof(struct rr_header_v2);
	else		else
	return -EINVAL;		return -EINVAL;