/*
 * usb_gcore_main.c
 * 
 * Copyright 2005,2006 Sony Corporation
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 * 
 */

/*-----------------------------------------------------------------------------
 * Include file
 *---------------------------------------------------------------------------*/
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/fs.h>
#include <linux/wait.h>
#include <linux/slab.h>
#include <linux/smp_lock.h>
#include <linux/proc_fs.h>
#include <linux/moduleparam.h>
#include <asm/uaccess.h>
#include <asm/errno.h>
#include <asm/types.h>
#include <linux/types.h>
#include <linux/device.h>
#include <linux/version.h>

#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 21)
#include <linux/usb/ch9.h>
#else
#include <linux/usb_ch9.h>
#endif

#include <linux/list.h>
#include <linux/usb_gadget.h>
#include <asm/arch/sonymisc.h>
#include <linux/usb/mcd/usb_mcd_ioctl.h>
#include <linux/usb/gcore/usb_event.h>
#include <linux/usb/gcore/usb_gadgetcore.h>
#include <linux/usb/gcore/usb_otgcore.h>

#include "usb_gadgetcore_cfg.h"
#include "usb_gadgetcore_pvt.h"

/*-----------------------------------------------------------------------------
 * Module infomation
 *---------------------------------------------------------------------------*/
MODULE_AUTHOR("Sony Corporation");
MODULE_DESCRIPTION(USBGADGETCORE_NAME
                   " driver ver " USBGADGETCORE_VERSION);
MODULE_LICENSE("GPL");

/*-----------------------------------------------------------------------------
 * Function prototype declaration
 *---------------------------------------------------------------------------*/
#if !((defined(CONFIG_ARCH_CXD4105BASED) || defined(CONFIG_ARCH_CXD4115BASED)) && \
      defined(CONFIG_USB_GADGET_CORE))
  #if !defined(CONFIG_USB_GADGET_CORE_MODULE)
   #error "Error: Kconfig"
  #endif
  static
#endif
int __init usb_gcore_module_init(void);

static void __exit usb_gcore_module_exit(void);
static int __init g_core_alloc(void);
static void g_core_free(struct g_core_drv*);
static int ioctl_probe(void*);
static int ioctl_remove(void*);
static int ioctl_start(void*);
static int ioctl_stop(void*);
static int usb_gadgetcore_ioctl(struct inode*, struct file*, unsigned int, unsigned long);
static int usb_gadgetcore_open(struct inode *inode, struct file*);
static int usb_gadgetcore_release(struct inode *inode, struct file*);

static int g_bind(struct usb_gadget*);
static void g_unbind(struct usb_gadget*);
static int g_setup(struct usb_gadget*, const struct usb_ctrlrequest*);
static void g_disconnect(struct usb_gadget*);
static void g_suspend(struct usb_gadget*);
static void g_resume(struct usb_gadget*);

/*-----------------------------------------------------------------------------
 * Variable declaration
 *---------------------------------------------------------------------------*/
static struct usb_gadget_driver g_core_driver;

static struct g_core_drv *the_g_core;

static struct usb_gadget_driver g_core_driver = 
{
    .speed      = USB_SPEED_HIGH,
    .function   = USBGADGETCORE_NAME,
    .bind       = g_bind,
    .unbind     = g_unbind,
    .setup      = g_setup,
    .disconnect = g_disconnect,
    .suspend    = g_suspend,
    .resume     = g_resume,
    .driver     = {
        .name       = USBGADGETCORE_NAME,
    },
};

/*=============================================================================
 *
 * Main function body
 *
 *===========================================================================*/
/*-----------------------------------------------------------------------------
 * ioctl実体
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
static int
ioctl_probe(void *arg)
{
    struct g_core_drv *g_core = the_g_core;
    struct usb_gadgetcore_probe_info tmp_info;
    unsigned long flags;
    unsigned long res;
    
    PDEBUG("%s call\n", __func__);
    
    if(!g_core){
        return -EINVAL;
    }
    
    /* User空間からtmpへコピー */
    res = copy_from_user(&tmp_info, arg, sizeof(tmp_info));
    if(res != 0){
        PWARN("error: probe failed\n");
        return -EINVAL;
    }
    
    /* 引数が正しいかを確認 */
    if(tmp_info.hndl == 0){
        PWARN("error: handle is 0\n");
        return -EINVAL;
    }
    
    spin_lock_irqsave(&g_core->lock, flags);
    
    /* すでにprobe済みか確認 */
    if(g_core->g_probe.hndl != 0){
        spin_unlock_irqrestore(&g_core->lock, flags);
        PWARN("error: probe failed\n");
        return -EBUSY;
    }
    
    /* tmpからコピー */
    memcpy(&g_core->g_probe, &tmp_info, sizeof(tmp_info));
    
    spin_unlock_irqrestore(&g_core->lock, flags);
    
    PDEBUG("g_probe.hndl  : %lu\n", g_core->g_probe.hndl);
    PDEBUG("g_probe.event : %lx\n",  (unsigned long)&g_core->g_probe.event);
    
    return 0;
}

/*-------------------------------------------------------------------------*/
static int
ioctl_remove(void *arg)
{
    struct g_core_drv *g_core = the_g_core;
    unsigned long flags;
    
    PDEBUG("%s call\n", __func__);
    
    if(!g_core){
        return -EINVAL;
    }
    
    /* Handleを0にする */
    spin_lock_irqsave(&g_core->lock, flags);
    g_core->g_probe.hndl = 0;
    spin_unlock_irqrestore(&g_core->lock, flags);
    
    return 0;
}

/*-------------------------------------------------------------------------*/
static int
ioctl_start(void *arg)
{
    struct g_core_drv *g_core = the_g_core;
    int res;
    
    PDEBUG("%s call\n", __func__);
    
    if(!g_core){
        return -EINVAL;
    }
    
    PDEBUG("&arg : 0x%08lx\n", (unsigned long)arg);
    
    /* すでにstart済みでないか確認 */
    if(test_and_set_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags) != 0){
        PWARN("start: Fail\n");
        return -EBUSY;
    }
    
    /* Gadget Controller へ regist する */
    PDEBUG("usb_gadget_register_driver()\n");
    res = usb_gadget_register_driver(&g_core_driver);
    if(res != 0){
        PERR("  -->failed\n");
        clear_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags);
        return res;
    }
    
    g_core->hs_disable = usb_otgcore_get_hs_disable();
    
    /* 受け取ったDescriptorTableを保存する */
    res = save_desc_tbl(g_core, (struct usb_gadgetcore_start_info*)arg);
    if(res != 0){
        PERR("Error : save_desc_tbl()\n");
        clear_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags);
        return res;
    }
    
#ifdef DESC_VERBOSE
    dump_device_desc(g_core->desc_tbl);
#endif /* DESC_VERBOSE */
    
    /* PullUP ON */
    res = usb_gadget_connect(g_core->gadget);
    if(res != 0){
        PERR("Error : usb_gadget_connect()\n");
        clear_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags);
    }
    return res;
}

/*-------------------------------------------------------------------------*/
static int
ioctl_stop(void *arg)
{
    struct g_core_drv *g_core = the_g_core;
    int res;
    
    PDEBUG("%s call\n", __func__);
    
    if(!g_core){
        return -EINVAL;
    }
    
    /* start済みフラグを確認、クリアする */
    if(test_and_clear_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags)){
        /* PullUP OFF */
        res = usb_gadget_disconnect(g_core->gadget);
        if(res != 0){
            PERR("Error : usb_gadget_disconnect()\n");
        }
        
        /* 動作中のFunctionDriverを終了する -> SetConfig(0)の処理 */
        setup_disconnect(g_core);
        
        /* DesciptorTableを破棄する */
        PDEBUG("free_desc_tbl()\n");
        free_desc_tbl(g_core);
        
        /* Gadget Controller から unregist する */
        res = usb_gadget_unregister_driver(&g_core_driver);
        if(res != 0){
            PERR("Error: Unregist from GadgetController\n");
        }
    }
    
    return 0;
}

/*-----------------------------------------------------------------------------
 * File Operations
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
static int
usb_gadgetcore_ioctl(struct inode *inode,
                      struct file *filp,
                      unsigned int cmd,
                      unsigned long arg)
{
    int res;
    
    switch(cmd){
      /*----------------------------------------*/
      case USB_IOC_GADGETCORE_PROBE:
        res = ioctl_probe((void*)arg);
        break;
        
      /*----------------------------------------*/
      case USB_IOC_GADGETCORE_REMOVE:
        res = ioctl_remove((char*)arg);
        break;
        
      /*----------------------------------------*/
      case USB_IOC_GADGETCORE_START:
        res = ioctl_start((void*)arg);
        break;
        
      /*----------------------------------------*/
      case USB_IOC_GADGETCORE_STOP:
        res = ioctl_stop((char*)arg);
        break;
        
      /*----------------------------------------*/
      default:
        PWARN("usb_gadgetcore_ioctl(none) call\n");
        res = -ENOTTY;
    }
    
    return res;
}

/*-------------------------------------------------------------------------*/
static int
usb_gadgetcore_open(struct inode *inode, struct file *filp)
{
    struct g_core_drv *g_core = the_g_core;
    
    /* 他のprocessがopenしていないか確認 */
    if(test_and_set_bit(USB_GCORE_ATOMIC_FD_LOCK, &g_core->atomic_bitflags) != 0){
        PWARN("usb_gadgetcore_open() failed\n");
        return -EBUSY;
    }
    
    PDEBUG("usb_gadgetcore_open() success\n");
    
    return 0;
}

/*-------------------------------------------------------------------------*/
static int
usb_gadgetcore_release(struct inode *inode, struct file *filp)
{
    struct g_core_drv *g_core = the_g_core;
    
    PDEBUG("usb_gadgetcore_release() success\n");
    
    /* 使用中を解除 */
    clear_bit(USB_GCORE_ATOMIC_FD_LOCK, &g_core->atomic_bitflags);
    
    return 0;
}

/*-----------------------------------------------------------------------------
 * File Operations 構造体
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
static struct file_operations usb_gadgetcore_fops = 
{
    .owner  = THIS_MODULE,
    .open   = usb_gadgetcore_open,
    .release = usb_gadgetcore_release,
    .ioctl  = usb_gadgetcore_ioctl,
};

static struct sonymisc_device usb_gadgetcore_device = 
{
    .class  = DEV_CLASS_CHR,
    .minor  = USBGADGETCORE_MINOR_NUMBER,
    .name   = USBGADGETCORE_NAME,
    .fops   = &usb_gadgetcore_fops,
    .gd     = NULL,
};

/*-----------------------------------------------------------------------------
 * ControllerからのCallback関数
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
static int
g_bind(struct usb_gadget *gadget)
{
    struct g_core_drv *g_core = the_g_core;
    struct g_ep_info        *ep_info;
    struct usb_ep           *ep;
    int rc = 0;
    
    PDEBUG("g_bind\n");
    PDEBUG("g_gadget : 0x%lx\n", (unsigned long)g_core);
    
    /* Controller から受け取った gadget構造体を保存 */
    g_core->gadget = gadget;
    
    set_gadget_data(gadget, g_core);
    g_core->ep0 = gadget->ep0;
    g_core->ep0->driver_data = g_core;
    
    /* EP0 用の request オブジェクトを取得 */
    g_core->ep0req = usb_ep_alloc_request(g_core->ep0, GFP_KERNEL);
    if(!g_core->ep0req){
        PDEBUG("usb_ep_alloc_request(EP0) ---> Fail\n");
        rc = -ENOMEM;
        goto exit;
    }
    PDEBUG("usb_ep_alloc_request(EP0) ---> Success\n");
    
    /* EP0 用の buffer を取得 */
    g_core->ep0req->buf = usb_ep_alloc_buffer(g_core->ep0, EP0_BUFSIZE, NULL, GFP_KERNEL);
    if(!g_core->ep0req->buf){
        PDEBUG("usb_ep_alloc_buffer(EP0) ---> Fail\n");
        rc = -ENOMEM;
        usb_ep_free_request(g_core->ep0, g_core->ep0req);
        goto exit;
    }
    PDEBUG("usb_ep_alloc_buffer(EP0) ---> Success\n");
    
    /* EP リストを初期化 */
    list_for_each_entry(ep, &gadget->ep_list, ep_list){
        ep_info = kmalloc(sizeof(*ep_info), GFP_KERNEL);
        PDEBUG(" ep_info: 0x%lx  cep: 0x%lx\n", (unsigned long)ep_info, (unsigned long)ep);
        if(!ep_info){
            rc = -ENOMEM;
        }else{
            ep_info->use = 0;
        }
        ep->driver_data = ep_info;
    }
    
    /* EP リストの初期化に失敗したときの処理 */
    if(rc != 0){
        PDEBUG("alloc ep_info ---> Fail\n");
        
        /* EP リストを開放 */
        list_for_each_entry(ep, &gadget->ep_list, ep_list){
            ep_info = ep->driver_data;
            if(!ep_info){
                continue;
            }
            PDEBUG("kfree ep_info: 0x%lx  cep: 0x%lx\n", (unsigned long)ep_info, (unsigned long)ep);
            kfree(ep_info);
        }
        
        /* EP0のリソースを開放 */
        usb_ep_free_buffer(g_core->ep0, g_core->ep0req->buf, (dma_addr_t)NULL, EP0_BUFSIZE);
        usb_ep_free_request(g_core->ep0, g_core->ep0req);
    }
    
exit:
    return rc;
}

/*-------------------------------------------------------------------------*/
static void
g_unbind(struct usb_gadget *gadget)
{
    struct g_core_drv  *g_core = get_gadget_data(gadget);
    struct usb_request *req = g_core->ep0req;
    struct g_ep_info        *ep_info;
    struct usb_ep           *ep;
    
    PDEBUG("g_unbind\n");
    
    /* EP0のリソースを開放 */
    if(req){
        if(req->buf){
            usb_ep_free_buffer(g_core->ep0, req->buf, req->dma, EP0_BUFSIZE);
        }
        usb_ep_free_request(g_core->ep0, req);
    }
    
    /* EP リストを開放 */
    list_for_each_entry(ep, &gadget->ep_list, ep_list){
        ep_info = ep->driver_data;
        PDEBUG("kfree ep_info: 0x%lx  cep: 0x%lx\n", (unsigned long)ep_info, (unsigned long)ep);
        if(!ep_info){
            continue;
        }
        kfree(ep_info);
    }
    
    set_gadget_data(gadget, NULL);
    
    return;
}

/*-------------------------------------------------------------------------*/
static int
g_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
{
    struct g_core_drv  *g_core = get_gadget_data(gadget);
    int rc;
    
    PDEBUG("setup() %02x.%02x v%04x i%04x\n",
              ctrl->bRequestType, ctrl->bRequest, ctrl->wValue, ctrl->wIndex);
    
    /* start済みフラグを確認する */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags)){
        /* setup処理にまわす */
        rc = setup_req(g_core, ctrl);
    }else{
        rc = 0;
        PDEBUG("g_setup() in stop state\n");
    }
    
    return rc;
}

/*-------------------------------------------------------------------------*/
static void g_disconnect(struct usb_gadget *gadget)
{
    struct g_core_drv  *g_core = get_gadget_data(gadget);
    
    PDEBUG("g_disconnect() <==\n");
    
    /* TestModeを終了させる */
    if(g_core->test_mode){
        stop_testmode(g_core);
        g_core->test_mode = 0;
    }
    
    /* start済みフラグを確認する */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags)){
        bus_disconnect(g_core);
    }else{
        PDEBUG("g_disconnect() in stop state\n");
    }
    
    PDEBUG("==>\n");
}

/*-------------------------------------------------------------------------*/
static void g_suspend(struct usb_gadget *gadget)
{
    struct g_core_drv  *g_core = get_gadget_data(gadget);
    
    PDEBUG("g_suspend() <==\n");
    
    set_bit(USB_GCORE_ATOMIC_SUSPENDED, &g_core->atomic_bitflags);
    
    /* start済みフラグを確認する */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags)){
        /* Busモジュールへ通知 */
        bus_suspend(g_core);
    }else{
        PDEBUG("g_suspend() in stop state\n");
    }
    
    PDEBUG("==>\n");
}

/*-------------------------------------------------------------------------*/
static void g_resume(struct usb_gadget *gadget)
{
    struct g_core_drv  *g_core = get_gadget_data(gadget);
    
    PDEBUG("g_resume()  <==\n");
    
    clear_bit(USB_GCORE_ATOMIC_SUSPENDED, &g_core->atomic_bitflags);
    
    /* start済みフラグを確認する */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags)){
        /* Busモジュールへ通知 */
        bus_resume(g_core);
    }else{
        PDEBUG("g_resume() in stop state\n");
    }
    
    PDEBUG("==>\n");
}

/*-----------------------------------------------------------------------------
 * module operation
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
static int __init
g_core_alloc(void)
{
    struct g_core_drv  *g_core;
    
    /* g_core 用の領域を取得 */
    g_core = kmalloc(sizeof *g_core, GFP_KERNEL);
    if(!g_core) return -ENOMEM;
    
    /* g_core を初期化 */
    memset(g_core, 0, sizeof *g_core);
    
    /* spin_lock用変数を初期化 */
    spin_lock_init(&g_core->lock);
    
    the_g_core = g_core;
    
    PDEBUG("the_g_core : 0x%lx\n", (unsigned long)the_g_core);
    
    return 0;
}

/*-------------------------------------------------------------------------*/
static void
g_core_free(struct g_core_drv *g_core)
{
    if(g_core != NULL){
        kfree(g_core);
    }
}

/*-------------------------------------------------------------------------*/
#if !((defined(CONFIG_ARCH_CXD4105BASED) || defined(CONFIG_ARCH_CXD4115BASED)) && \
      defined(CONFIG_USB_GADGET_CORE))
  #if !defined(CONFIG_USB_GADGET_CORE_MODULE)
   #error "Error: Kconfig"
  #endif
static
#endif
int __init usb_gcore_module_init(void)
{
    struct g_core_drv *g_core;
    int res;
    
    PINFO("USB GadgetCore driver ver " USBGADGETCORE_VERSION "\n");
    
    /* g_core_drv 用の領域を確保 */
    res = g_core_alloc();
    if(res != 0){
        return res;
    }
    
    /* Gadget Core Driver 実体を取得 */
    g_core = the_g_core;
    
    /* FunctionDriverList を初期化する */
    INIT_LIST_HEAD(&g_core->func_list);
    
    /* sonymiscにCharacterDeviceとして登録 */
    res = sonymisc_register( &usb_gadgetcore_device );
    
    clear_bit(USB_GCORE_ATOMIC_FD_LOCK, &g_core->atomic_bitflags);
    
    if(res == 0){
        PDEBUG("usb_gcore_module_init() success\n");
    }else{
        printk(" -->fail!!\n");
        PDEBUG("usb_gcore_module_init() failed\n");
    }
    
    return 0;
}

/*-------------------------------------------------------------------------*/
static
void __exit usb_gcore_module_exit(void)
{
    int res;
    struct g_core_drv *g_core = the_g_core;
    
    /* sonymiscから登録削除 */
    res = sonymisc_deregister( &usb_gadgetcore_device );
    if(res != 0){
        PERR("Error: Unregist from sonymisc\n");
    }
    
    /* start済みフラグを確認、クリアする */
    if(test_and_clear_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags)){
        /* PullUP OFF */
        res = usb_gadget_disconnect(g_core->gadget);
        if(res != 0){
            PERR("Error : usb_gadget_disconnect()\n");
        }
        
        /* 動作中のFunctionDriverを終了する -> SetConfig(0)の処理 */
        setup_disconnect(g_core);
        
        /* DesciptorTableを破棄する */
        PDEBUG("free_desc_tbl()\n");
        free_desc_tbl(g_core);
        
        /* Gadget Controller から unregist する */
        res = usb_gadget_unregister_driver(&g_core_driver);
        if(res != 0){
            PERR("Error: Unregist from GadgetController\n");
        }
    }
    
    /* g_core構造体を開放 */
    g_core_free(g_core);
}

/*-----------------------------------------------------------------------------
 * TestMode
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
int start_testmode(struct g_core_drv *g_core, u8 testmode)
{
    int rc;
    
    switch(testmode){
      case 1:
        PDEBUG("TestMode: TEST_J\n");
        rc = usb_gadget_ioctl(g_core->gadget, 
                              USB_IOCTL_TESTMODE, 
                              USB_IOCTL_TESTMODE_TEST_J);
        break;
      case 2:
        PDEBUG("TestMode: TEST_K\n");
        rc = usb_gadget_ioctl(g_core->gadget, 
                              USB_IOCTL_TESTMODE, 
                              USB_IOCTL_TESTMODE_TEST_K);
        break;
      case 3:
        PDEBUG("TestMode: SE0_NAK\n");
        rc = usb_gadget_ioctl(g_core->gadget, 
                              USB_IOCTL_TESTMODE, 
                              USB_IOCTL_TESTMODE_SE0_NAK);
        break;
      case 4:
        PDEBUG("TestMode: TEST_PACKET\n");
        rc = usb_gadget_ioctl(g_core->gadget, 
                              USB_IOCTL_TESTMODE, 
                              USB_IOCTL_TESTMODE_TEST_PACKET);
        break;
      default:
        PWARN("TestMode: Error\n");
        rc = -EINVAL;
        break;
    }
    
    return rc;
}

/*-------------------------------------------------------------------------*/
int stop_testmode(struct g_core_drv *g_core)
{
    int rc;
    
    PDEBUG("TestMode: Stop!!\n");
    rc = usb_gadget_ioctl(g_core->gadget, 
                          USB_IOCTL_TESTMODE,
                          USB_IOCTL_TESTMODE_NORMAL);
    return rc;
}

/*-----------------------------------------------------------------------------
 * EXPORT_SYMBOL function
 *---------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*/
int
usb_gadgetcore_register_driver(struct usb_gadget_func_driver *func_drv)
{
    struct g_core_drv *g_core = the_g_core;
    struct g_func_drv *g_func_drv, *tmp_func_drv;
    unsigned long flags;
    
    PDEBUG("%s call\n", __func__);
    
    /* GadgetCoreがstart済みでないか確認 */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags) != 0){
        PWARN("usb_gadgetcore_register_driver(): Fail\n");
        return -EBUSY;
    }
    
    /* 必要なメンバが定義されているか確認 */
    if(!func_drv        ||
       !func_drv->start ||
       !func_drv->stop     ){
        return -EINVAL;
    }
    
    PDEBUG("========================================================\n");
    PDEBUG("FunctionDriver(%s) Added\n", func_drv->function ? func_drv->function : "");
    PDEBUG(" : 0x%lx\n", (unsigned long)func_drv);
    PDEBUG(" Config: %d Interface: %d\n", func_drv->config, func_drv->interface);
    PDEBUG(" start():         0x%lx\n", (unsigned long)func_drv->start);
    PDEBUG(" stop():          0x%lx\n", (unsigned long)func_drv->stop);
    PDEBUG(" ep_set_halt():   0x%lx\n", (unsigned long)func_drv->ep_set_halt);
    PDEBUG(" ep_clear_halt(): 0x%lx\n", (unsigned long)func_drv->ep_clear_halt);
    PDEBUG(" suspend():       0x%lx\n", (unsigned long)func_drv->suspend);
    PDEBUG(" resume():        0x%lx\n", (unsigned long)func_drv->resume);
    PDEBUG(" class():         0x%lx\n", (unsigned long)func_drv->class);
    PDEBUG(" vendor():        0x%lx\n", (unsigned long)func_drv->vendor);
    PDEBUG("========================================================\n");
    
    g_func_drv = (struct g_func_drv*)kmalloc(sizeof(*g_func_drv), GFP_KERNEL);
    if(!g_func_drv){
        PWARN("kmalloc(): Fail\n");
        return -EINVAL;
    }
    
    PDEBUG("g_func_drv: 0x%lx\n", (unsigned long)g_func_drv);
    g_func_drv->func_drv = func_drv;
    g_func_drv->started = 0;
    
    spin_lock_irqsave(&g_core->lock, flags);
    
    /* FunctionDriverがリストに無いことを確認する */
    list_for_each_entry(tmp_func_drv, &g_core->func_list, list){
        if(tmp_func_drv->func_drv == g_func_drv->func_drv){
            /* すでに指定のFunctionDriverが登録されていたら エラーを返す */
            spin_unlock_irqrestore(&g_core->lock, flags);
            kfree(g_func_drv);
            return -EINVAL;
        }
    }
    
    /* FunctionDriverをリストに追加 */
    list_add_tail(&g_func_drv->list, &g_core->func_list);
    
    spin_unlock_irqrestore(&g_core->lock, flags);
    
    /* FunctionDriverを表示 */
    PDEBUG("Print Func Driver :\n");
    list_for_each_entry(tmp_func_drv, &g_core->func_list, list){
        PDEBUG("     --> Cfg: %d In: %d %s\n", tmp_func_drv->func_drv->config,
                                               tmp_func_drv->func_drv->interface,
                                               tmp_func_drv->func_drv->function ? 
                                               tmp_func_drv->func_drv->function : "");
    }
    
    return 0;
}

/*-------------------------------------------------------------------------*/
int
usb_gadgetcore_unregister_driver(struct usb_gadget_func_driver *func_drv)
{
    struct g_core_drv *g_core = the_g_core;
    struct g_func_drv *tmp_func_drv, *n;
    unsigned long flags;
    
    PDEBUG("%s call\n", __func__);
    
    /* GadgetCoreがstart済みでないか確認 */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags) != 0){
        PWARN("usb_gadgetcore_unregister_driver(): Fail\n");
        return -EBUSY;
    }
    
    spin_lock_irqsave(&g_core->lock, flags);
    
    /* リストから指定のFunctionDriverを探す */
    list_for_each_entry_safe(tmp_func_drv, n, &g_core->func_list, list){
        if(tmp_func_drv->func_drv == func_drv){
            /* 指定のFunctionDriverが見つかったらリストから削除 */
            PDEBUG("del func_drv: 0x%lx\n", (unsigned long)tmp_func_drv->func_drv);
            PDEBUG("tmp_func_drv: 0x%lx\n", (unsigned long)tmp_func_drv);
            list_del(&tmp_func_drv->list);
            kfree(tmp_func_drv);
        }
    }
    
    spin_unlock_irqrestore(&g_core->lock, flags);
    
    return 0;
}

/*-------------------------------------------------------------------------*/
int
usb_gadgetcore_stop_other_driver(struct usb_gadget_func_driver *func_drv)
{
    struct g_core_drv *g_core = the_g_core;
    struct g_func_drv *tmp_func_drv, *n;
    struct usb_ep **eps = NULL;
    unsigned long flags;
    u8 i;
    
    PDEBUG("%s(%lx)\n", __func__, (unsigned long)func_drv);
    
    /* GadgetCoreがstart済みでないならば正常終了 */
    if(test_bit(USB_GCORE_ATOMIC_START, &g_core->atomic_bitflags) == 0){
        PWARN("usb_gadgetcore_stop_other_driver(): Fail\n");
        return 0;
    }
    
    /* スピンロック */
    spin_lock_irqsave(&g_core->lock, flags);
    
    /* リストから指定のFunctionDriver以外を探す */
    list_for_each_entry_safe(tmp_func_drv, n, &g_core->func_list, list){
        if(tmp_func_drv->func_drv != func_drv   &&
           tmp_func_drv->started != 0                 ){
            /* ここで条件の一致するFuncDrvへstop()通知を送る */
            PVERBOSE(" stop() to %s\n", 
                     tmp_func_drv->func_drv->function ?
                     tmp_func_drv->func_drv->function : "");
            tmp_func_drv->started = 0;
            tmp_func_drv->func_drv->stop(tmp_func_drv->func_drv);
            
            eps = tmp_func_drv->ep_list.ep;
            
            /* FuncDrv用EPリストを削除 */
            for(i = 0; i < tmp_func_drv->ep_list.num_ep; i++){
                ep_ctrl_delete_ep(g_core, *(eps+i));
            }
            kfree(eps);
            
        }else{
            PVERBOSE(" skip: %s\n", 
                     tmp_func_drv->func_drv->function ?
                     tmp_func_drv->func_drv->function : "");
        }
    }
    
    /* スピンロック解除 */
    spin_unlock_irqrestore(&g_core->lock, flags);
    
    return 0;
}

/*=============================================================================
 * Export symbols
 *===========================================================================*/
EXPORT_SYMBOL(usb_gadgetcore_register_driver);
EXPORT_SYMBOL(usb_gadgetcore_unregister_driver);
EXPORT_SYMBOL(usb_gadgetcore_stop_other_driver);

#if !((defined(CONFIG_ARCH_CXD4105BASED) || defined(CONFIG_ARCH_CXD4115BASED)) && \
      defined(CONFIG_USB_GADGET_CORE))
  #if !defined(CONFIG_USB_GADGET_CORE_MODULE)
   #error "Error: Kconfig"
  #endif
  module_init(usb_gcore_module_init);
#endif
module_exit(usb_gcore_module_exit);

