usb: gadget: nokia: Add mass storage driver to g_nokia

This patch adds removable mass storage support to g_nokia gadget (for N900).
It means that at runtime block device can be exported or unexported.
So it does not export anything by default and thus allows to use MyDocs
partition as before...

[ balbi@ti.com: make it build ]

Signed-off-by: Pali Rohár <pali.rohar@gmail.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

diff --git a/drivers/usb/gadget/legacy/Kconfig b/drivers/usb/gadget/legacy/Kconfig
index d5a7102de696611ea45124796610a20f9fb033cd..ddef41f6df3e567b1a6c00ed02f3006b32ac362f 100644 (file)
--- a/drivers/usb/gadget/legacy/Kconfig
+++ b/drivers/usb/gadget/legacy/Kconfig
@@ -346,6 +346,7 @@ config USB_G_NOKIA
        select USB_F_OBEX
        select USB_F_PHONET
        select USB_F_ECM
+       select USB_F_MASS_STORAGE
        help
          The Nokia composite gadget provides support for acm, obex
          and phonet in only one composite gadget driver.

diff --git a/drivers/usb/gadget/legacy/nokia.c b/drivers/usb/gadget/legacy/nokia.c
index 4bb498a38a1c01eb17077844bc163028c3e22b93..e3791b91999627bdad4ed2703ba6a572e9024d1c 100644 (file)
--- a/drivers/usb/gadget/legacy/nokia.c
+++ b/drivers/usb/gadget/legacy/nokia.c
@@ -24,6 +24,7 @@
 #include "u_phonet.h"
 #include "u_ecm.h"
 #include "gadget_chips.h"
+#include "f_mass_storage.h"
 
 /* Defines */
 
@@ -34,6 +35,29 @@ USB_GADGET_COMPOSITE_OPTIONS();
 
 USB_ETHERNET_MODULE_PARAMETERS();
 
+static struct fsg_module_parameters fsg_mod_data = {
+       .stall = 0,
+       .luns = 2,
+       .removable_count = 2,
+       .removable = { 1, 1, },
+};
+
+#ifdef CONFIG_USB_GADGET_DEBUG_FILES
+
+static unsigned int fsg_num_buffers = CONFIG_USB_GADGET_STORAGE_NUM_BUFFERS;
+
+#else
+
+/*
+ * Number of buffers we will use.
+ * 2 is usually enough for good buffering pipeline
+ */
+#define fsg_num_buffers        CONFIG_USB_GADGET_STORAGE_NUM_BUFFERS
+
+#endif /* CONFIG_USB_DEBUG */
+
+FSG_MODULE_PARAMETERS(/* no prefix */, fsg_mod_data);
+
 #define NOKIA_VENDOR_ID                        0x0421  /* Nokia */
 #define NOKIA_PRODUCT_ID               0x01c8  /* Nokia Gadget */
 
@@ -94,6 +118,8 @@ static struct usb_function *f_obex1_cfg2;
 static struct usb_function *f_obex2_cfg2;
 static struct usb_function *f_phonet_cfg1;
 static struct usb_function *f_phonet_cfg2;
+static struct usb_function *f_msg_cfg1;
+static struct usb_function *f_msg_cfg2;
 
 
 static struct usb_configuration nokia_config_500ma_driver = {
@@ -117,6 +143,7 @@ static struct usb_function_instance *fi_ecm;
 static struct usb_function_instance *fi_obex1;
 static struct usb_function_instance *fi_obex2;
 static struct usb_function_instance *fi_phonet;
+static struct usb_function_instance *fi_msg;
 
 static int nokia_bind_config(struct usb_configuration *c)
 {
@@ -125,6 +152,8 @@ static int nokia_bind_config(struct usb_configuration *c)
        struct usb_function *f_obex1 = NULL;
        struct usb_function *f_ecm;
        struct usb_function *f_obex2 = NULL;
+       struct usb_function *f_msg;
+       struct fsg_opts *fsg_opts;
        int status = 0;
        int obex1_stat = -1;
        int obex2_stat = -1;
@@ -160,6 +189,12 @@ static int nokia_bind_config(struct usb_configuration *c)
                goto err_get_ecm;
        }
 
+       f_msg = usb_get_function(fi_msg);
+       if (IS_ERR(f_msg)) {
+               status = PTR_ERR(f_msg);
+               goto err_get_msg;
+       }
+
        if (!IS_ERR_OR_NULL(f_phonet)) {
                phonet_stat = usb_add_function(c, f_phonet);
                if (phonet_stat)
@@ -187,21 +222,36 @@ static int nokia_bind_config(struct usb_configuration *c)
                pr_debug("could not bind ecm config %d\n", status);
                goto err_ecm;
        }
+
+       fsg_opts = fsg_opts_from_func_inst(fi_msg);
+
+       status = fsg_common_run_thread(fsg_opts->common);
+       if (status)
+               goto err_msg;
+
+       status = usb_add_function(c, f_msg);
+       if (status)
+               goto err_msg;
+
        if (c == &nokia_config_500ma_driver) {
                f_acm_cfg1 = f_acm;
                f_ecm_cfg1 = f_ecm;
                f_phonet_cfg1 = f_phonet;
                f_obex1_cfg1 = f_obex1;
                f_obex2_cfg1 = f_obex2;
+               f_msg_cfg1 = f_msg;
        } else {
                f_acm_cfg2 = f_acm;
                f_ecm_cfg2 = f_ecm;
                f_phonet_cfg2 = f_phonet;
                f_obex1_cfg2 = f_obex1;
                f_obex2_cfg2 = f_obex2;
+               f_msg_cfg2 = f_msg;
        }
 
        return status;
+err_msg:
+       usb_remove_function(c, f_ecm);
 err_ecm:
        usb_remove_function(c, f_acm);
 err_conf:
@@ -211,6 +261,8 @@ err_conf:
                usb_remove_function(c, f_obex1);
        if (!phonet_stat)
                usb_remove_function(c, f_phonet);
+       usb_put_function(f_msg);
+err_get_msg:
        usb_put_function(f_ecm);
 err_get_ecm:
        usb_put_function(f_acm);
@@ -227,6 +279,8 @@ err_get_acm:
 static int nokia_bind(struct usb_composite_dev *cdev)
 {
        struct usb_gadget       *gadget = cdev->gadget;
+       struct fsg_opts         *fsg_opts;
+       struct fsg_config       fsg_config;
        int                     status;
 
        status = usb_string_ids_tab(cdev, strings_dev);
@@ -267,11 +321,46 @@ static int nokia_bind(struct usb_composite_dev *cdev)
                goto err_acm_inst;
        }
 
+       fi_msg = usb_get_function_instance("mass_storage");
+       if (IS_ERR(fi_msg)) {
+               status = PTR_ERR(fi_msg);
+               goto err_ecm_inst;
+       }
+
+       /* set up mass storage function */
+       fsg_config_from_params(&fsg_config, &fsg_mod_data, fsg_num_buffers);
+       fsg_config.vendor_name = "Nokia";
+       fsg_config.product_name = "N900";
+
+       fsg_opts = fsg_opts_from_func_inst(fi_msg);
+       fsg_opts->no_configfs = true;
+
+       status = fsg_common_set_num_buffers(fsg_opts->common, fsg_num_buffers);
+       if (status)
+               goto err_msg_inst;
+
+       status = fsg_common_set_nluns(fsg_opts->common, fsg_config.nluns);
+       if (status)
+               goto err_msg_buf;
+
+       status = fsg_common_set_cdev(fsg_opts->common, cdev, fsg_config.can_stall);
+       if (status)
+               goto err_msg_set_nluns;
+
+       fsg_common_set_sysfs(fsg_opts->common, true);
+
+       status = fsg_common_create_luns(fsg_opts->common, &fsg_config);
+       if (status)
+               goto err_msg_set_nluns;
+
+       fsg_common_set_inquiry_string(fsg_opts->common, fsg_config.vendor_name,
+                                     fsg_config.product_name);
+
        /* finally register the configuration */
        status = usb_add_config(cdev, &nokia_config_500ma_driver,
                        nokia_bind_config);
        if (status < 0)
-               goto err_ecm_inst;
+               goto err_msg_set_cdev;
 
        status = usb_add_config(cdev, &nokia_config_100ma_driver,
                        nokia_bind_config);
@@ -292,6 +381,14 @@ err_put_cfg1:
        if (!IS_ERR_OR_NULL(f_phonet_cfg1))
                usb_put_function(f_phonet_cfg1);
        usb_put_function(f_ecm_cfg1);
+err_msg_set_cdev:
+       fsg_common_remove_luns(fsg_opts->common);
+err_msg_set_nluns:
+       fsg_common_free_luns(fsg_opts->common);
+err_msg_buf:
+       fsg_common_free_buffers(fsg_opts->common);
+err_msg_inst:
+       usb_put_function_instance(fi_msg);
 err_ecm_inst:
        usb_put_function_instance(fi_ecm);
 err_acm_inst:
@@ -325,7 +422,10 @@ static int nokia_unbind(struct usb_composite_dev *cdev)
        usb_put_function(f_acm_cfg2);
        usb_put_function(f_ecm_cfg1);
        usb_put_function(f_ecm_cfg2);
+       usb_put_function(f_msg_cfg1);
+       usb_put_function(f_msg_cfg2);
 
+       usb_put_function_instance(fi_msg);
        usb_put_function_instance(fi_ecm);
        if (!IS_ERR(fi_obex2))
                usb_put_function_instance(fi_obex2);