[PATCH] usb: gadget: f_fs: add the support for alternate interface setting

[Parikshit Pareek]

In FFS the support for multiple alternate interface settings are not present.
This patch implements the due support. The user application is required to
write interface descriptors to ep0 file of function fs, one for each alternate
interface setting of the same interface, with bAlternateSetting set to different
alt setting values. The order of interface descriptors associated with different
alternate interface settings is same as mentioned by the usb standard. For
example, interface descriptor with alt-setting as 0, followed by it's endpoint
descriptors, then interface descriptor of alt-setting 1, followed by endpoint
descriptors of this alternate interface settings, and so on.
Since alternate setting support is implemented, ffs_func_get_alt function is
also implemented to get alternate interface settings.

Signed-off-by: Parikshit Pareek <purohit4891@xxxxxxxxx>
---
 drivers/usb/gadget/function/f_fs.c | 304 ++++++++++++++++++++++++++++++-------
 1 file changed, 253 insertions(+), 51 deletions(-)

diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index 3ada83d..1986c51 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -88,7 +88,6 @@ static struct ffs_function *ffs_func_from_usb(struct usb_function *f)
 
 
 static void ffs_func_eps_disable(struct ffs_function *func);
-static int __must_check ffs_func_eps_enable(struct ffs_function *func);
 
 static int ffs_func_bind(struct usb_configuration *,
 			 struct usb_function *);
@@ -1868,46 +1867,6 @@ static void ffs_func_eps_disable(struct ffs_function *func)
 	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
 }
 
-static int ffs_func_eps_enable(struct ffs_function *func)
-{
-	struct ffs_data *ffs      = func->ffs;
-	struct ffs_ep *ep         = func->eps;
-	struct ffs_epfile *epfile = ffs->epfiles;
-	unsigned count            = ffs->eps_count;
-	unsigned long flags;
-	int ret = 0;
-
-	spin_lock_irqsave(&func->ffs->eps_lock, flags);
-	while(count--) {
-		ep->ep->driver_data = ep;
-
-		ret = config_ep_by_speed(func->gadget, &func->function, ep->ep);
-		if (ret) {
-			pr_err("%s: config_ep_by_speed(%s) returned %d\n",
-					__func__, ep->ep->name, ret);
-			break;
-		}
-
-		ret = usb_ep_enable(ep->ep);
-		if (likely(!ret)) {
-			epfile->ep = ep;
-			epfile->in = usb_endpoint_dir_in(ep->ep->desc);
-			epfile->isoc = usb_endpoint_xfer_isoc(ep->ep->desc);
-		} else {
-			break;
-		}
-
-		++ep;
-		++epfile;
-	}
-
-	wake_up_interruptible(&ffs->wait);
-	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
-
-	return ret;
-}
-
-
 /* Parsing and building descriptors and strings *****************************/
 
 /*
@@ -1966,7 +1925,10 @@ static int __must_check ffs_do_single_desc(char *data, unsigned len,
 			pr_vdebug("invalid entity's value\n");		\
 			return -EINVAL;					\
 		}							\
-		ret = entity(FFS_ ##type, &val, _ds, priv);		\
+		if (entity)						\
+			ret = entity(FFS_ ##type, &val, _ds, priv);	\
+		else							\
+			ret = 0;					\
 		if (unlikely(ret < 0)) {				\
 			pr_debug("entity " #type "(%02x); ret = %d\n",	\
 				 (val), ret);				\
@@ -2100,6 +2062,67 @@ static int __must_check ffs_do_descs(unsigned count, char *data, unsigned len,
 	}
 }
 
+static int do_function_enable_interface(enum ffs_entity_type type, u8 *valuep,
+					struct usb_descriptor_header *desc,
+					void *priv);
+
+static int do_function_disable_interface(enum ffs_entity_type type, u8 *valuep,
+					struct usb_descriptor_header *desc,
+					void *priv);
+
+static int __must_check ffs_do_descs_alt_intf(unsigned int count, char *data,
+					unsigned int len, unsigned int intf,
+					unsigned int alt, void *priv)
+{
+	const unsigned int _len = len;
+	unsigned long num = 0;
+	struct usb_descriptor_header *_ds;
+	struct usb_interface_descriptor *idecs;
+	ffs_entity_callback entity = NULL;
+
+	ENTER();
+
+	for (;;) {
+		int ret;
+
+		if (num == count)
+			data = NULL;
+
+		if (!data) {
+			pr_vdebug("%s Exiting. No more Data\n", __func__);
+			return _len - len;
+		}
+
+		_ds = (void *)data;
+		if (_ds->bDescriptorType == USB_DT_INTERFACE) {
+			idecs = (void *)_ds;
+
+			/*Check the interface no to deal with */
+			if (idecs->bInterfaceNumber == intf) {
+				if (idecs->bAlternateSetting == alt)
+					entity = do_function_enable_interface;
+				else
+					entity = do_function_disable_interface;
+			} else if (entity &&
+					(idecs->bInterfaceNumber != intf)) {
+				pr_vdebug(
+				"%s Exiting.Moved past the interface no\n",
+								__func__);
+				return _len - len;
+			}
+		}
+		ret = ffs_do_single_desc(data, len, entity, priv);
+		if (unlikely(ret < 0)) {
+			pr_err("%s Exiting with err %d\n", __func__, ret);
+			return ret;
+		}
+
+		len -= ret;
+		data += ret;
+		++num;
+	}
+}
+
 static int __ffs_data_do_entity(enum ffs_entity_type type,
 				u8 *valuep, struct usb_descriptor_header *desc,
 				void *priv)
@@ -2805,16 +2828,33 @@ static int __ffs_func_bind_do_nums(enum ffs_entity_type type, u8 *valuep,
 		/* Handled in previous pass by __ffs_func_bind_do_descs() */
 		return 0;
 
-	case FFS_INTERFACE:
+	case FFS_INTERFACE: {
+
+		struct usb_interface_descriptor *ds = (void *)desc;
+		int id = -EINVAL;
+
 		idx = *valuep;
-		if (func->interfaces_nums[idx] < 0) {
-			int id = usb_interface_id(func->conf, &func->function);
-			if (unlikely(id < 0))
+
+		if (!ds->bAlternateSetting &&
+			(func->interfaces_nums[idx] < 0)) {
+			id = usb_interface_id(func->conf,
+					&func->function);
+
+			if (unlikely(id < 0)) {
+				pr_err("%s wrong intf value allocated%d\n",
+								__func__, id);
 				return id;
+			}
 			func->interfaces_nums[idx] = id;
+		} else if (func->interfaces_nums[idx]  < 0) {
+			pr_err("%s Intf not assigned for alt %d intf no %d\n",
+					__func__, ds->bAlternateSetting, idx);
+			return id;
 		}
+
 		newValue = func->interfaces_nums[idx];
 		break;
+	}
 
 	case FFS_STRING:
 		/* String' IDs are allocated when fsf_data is bound to cdev */
@@ -2925,6 +2965,107 @@ static int __ffs_func_bind_do_os_desc(enum ffs_os_desc_type type,
 	return length;
 }
 
+/* Enables the endpint by it's Endpointaddress.
+ * We need to selectively enable the endpoints
+ * belonging to a particular interface setting.
+ */
+static int ffs_func_ep_en_by_epadd(struct ffs_function *func,
+					int ep_add, int enable)
+{
+	struct ffs_data *ffs      = func->ffs;
+	struct ffs_ep *ep         = func->eps;
+	struct ffs_epfile *epfile = ffs->epfiles;
+	unsigned int count            = ffs->eps_count;
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&func->ffs->eps_lock, flags);
+	do {
+		struct usb_endpoint_descriptor *ds;
+		int desc_idx;
+
+		if (ffs->gadget->speed == USB_SPEED_SUPER)
+			desc_idx = 2;
+		else if (ffs->gadget->speed == USB_SPEED_HIGH)
+			desc_idx = 1;
+		else
+			desc_idx = 0;
+
+		/* fall-back to lower speed if desc missing for current speed */
+		do {
+			ds = ep->descs[desc_idx];
+		} while (!ds && --desc_idx >= 0);
+
+		if (!ds) {
+			spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
+			return -EINVAL;
+		}
+
+		if (ep_add == ds->bEndpointAddress) {
+			if (enable) {
+
+				ep->ep->driver_data = ep;
+				ep->ep->desc = ds;
+				ret = usb_ep_enable(ep->ep);
+				if (likely(!ret)) {
+					epfile->ep = ep;
+					epfile->in = usb_endpoint_dir_in(ds);
+					epfile->isoc =
+						usb_endpoint_xfer_isoc(ds);
+				}
+				wake_up_interruptible(&ffs->wait);
+			}
+
+			else {
+				/* pending requests get nuked */
+				if (likely(ep->ep))
+					usb_ep_disable(ep->ep);
+
+				if (epfile)
+					epfile->ep = NULL;
+			}
+			break;
+		}
+		++ep;
+		++epfile;
+	} while (--count);
+	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
+
+	return ret;
+}
+
+
+static int do_function_en_dis_interface(enum ffs_entity_type type, u8 *valuep,
+					struct usb_descriptor_header *desc,
+					void *priv, int enable)
+{
+	struct usb_endpoint_descriptor *ds = NULL;
+	struct ffs_function *func = priv;
+	int ret = 0;
+
+	if (type != FFS_ENDPOINT)
+		return 0;
+
+	ds = (void *)desc;
+	pr_vdebug("%s EP %s ds %p\n", enable ? "enabled" : "disabled", ds);
+	ret = ffs_func_ep_en_by_epadd(func, ds->bEndpointAddress, enable);
+	return ret;
+}
+
+static int do_function_enable_interface(enum ffs_entity_type type, u8 *valuep,
+					struct usb_descriptor_header *desc,
+					void *priv)
+{
+	return do_function_en_dis_interface(type, valuep, desc, priv, true);
+}
+
+static int do_function_disable_interface(enum ffs_entity_type type, u8 *valuep,
+					struct usb_descriptor_header *desc,
+					void *priv)
+{
+	return do_function_en_dis_interface(type, valuep, desc, priv, false);
+}
+
 static inline struct f_fs_opts *ffs_do_functionfs_bind(struct usb_function *f,
 						struct usb_configuration *c)
 {
@@ -3155,6 +3296,19 @@ static int ffs_func_bind(struct usb_configuration *c,
 
 /* Other USB function hooks *************************************************/
 
+/* Clear the previous alt setting mapped from BIT8-15.
+ * Let the Interface no remain intact in BIT0-7
+ */
+#define FFS_SET_INTF_ALT_SET(intf, alt) do {		\
+	if (intf > -1) {				\
+		func->interfaces_nums[intf]  &= 0xFF;	\
+		func->interfaces_nums[intf] =		\
+			func->interfaces_nums[intf] |	\
+			(short)((alt & 0xff) << 8);	\
+	}						\
+}							\
+while (0)
+
 static void ffs_reset_work(struct work_struct *work)
 {
 	struct ffs_data *ffs = container_of(work,
@@ -3162,12 +3316,32 @@ static void ffs_reset_work(struct work_struct *work)
 	ffs_data_reset(ffs);
 }
 
+static int ffs_func_get_alt(struct usb_function *f,
+			unsigned int interface)
+{
+
+	int intf;
+	struct ffs_function *func = ffs_func_from_usb(f);
+
+	intf = ffs_func_revmap_intf(func, interface);
+
+	if (unlikely(intf < 0))
+		return intf;
+
+	return (func->interfaces_nums[intf] >> 8) & 0xff;
+}
+
+
 static int ffs_func_set_alt(struct usb_function *f,
 			    unsigned interface, unsigned alt)
 {
 	struct ffs_function *func = ffs_func_from_usb(f);
 	struct ffs_data *ffs = func->ffs;
-	int ret = 0, intf;
+	int ret = 0, intf = -1;
+	unsigned int count = 0;
+	struct usb_descriptor_header  **desc_p = NULL;
+
+	pr_vdebug("%s intf %d alt %d\n", __func__, interface, alt);
 
 	if (alt != (unsigned)-1) {
 		intf = ffs_func_revmap_intf(func, interface);
@@ -3175,7 +3349,7 @@ static int ffs_func_set_alt(struct usb_function *f,
 			return intf;
 	}
 
-	if (ffs->func)
+	if (ffs->func && ((alt == -1) || (ffs->state != FFS_ACTIVE)))
 		ffs_func_eps_disable(ffs->func);
 
 	if (ffs->state == FFS_DEACTIVATED) {
@@ -3195,9 +3369,36 @@ static int ffs_func_set_alt(struct usb_function *f,
 	}
 
 	ffs->func = func;
-	ret = ffs_func_eps_enable(func);
-	if (likely(ret >= 0))
+	if (ffs->gadget->speed == USB_SPEED_SUPER) {
+		count = ffs->ss_descs_count;
+		desc_p = func->function.ss_descriptors;
+	}
+	if (!count && !desc_p && ffs->gadget->speed == USB_SPEED_HIGH) {
+		count = ffs->hs_descs_count;
+		desc_p = func->function.hs_descriptors;
+	}
+	if (!count && !desc_p && ffs->gadget->speed == USB_SPEED_FULL) {
+		count = ffs->fs_descs_count;
+		desc_p = func->function.fs_descriptors;
+	}
+
+	if (desc_p && count)
+		ret = ffs_do_descs_alt_intf(count, (void *)desc_p[0],
+		ffs->raw_descs_length, interface, alt, func);
+	else
+		ret = -ENODEV;
+
+	if (unlikely(ret < 0))
+		pr_err("%s err %d\n", __func__, ret);
+	else
+		ret = 0;
+
+	if (likely(ret >= 0)) {
+
+		FFS_SET_INTF_ALT_SET(intf, alt);
 		ffs_event_add(ffs, FUNCTIONFS_ENABLE);
+	}
+
 	return ret;
 }
 
@@ -3315,7 +3516,7 @@ static int ffs_func_revmap_intf(struct ffs_function *func, u8 intf)
 	unsigned count = func->ffs->interfaces_count;
 
 	for (; count; --count, ++nums) {
-		if (*nums >= 0 && *nums == intf)
+		if (((*nums & 0xff) >= 0) && ((*nums & 0xff) == intf))
 			return nums - func->interfaces_nums;
 	}
 
@@ -3506,6 +3707,7 @@ static struct usb_function *ffs_alloc(struct usb_function_instance *fi)
 	func->function.bind    = ffs_func_bind;
 	func->function.unbind  = ffs_func_unbind;
 	func->function.set_alt = ffs_func_set_alt;
+	func->function.get_alt = ffs_func_get_alt;
 	func->function.disable = ffs_func_disable;
 	func->function.setup   = ffs_func_setup;
 	func->function.req_match = ffs_func_req_match;
-- 
1.9.1