- /* Recover OTG control */
- r = musb_ulpi_readb(musb->mregs, ISP1704_OTG_CTRL);
- r |= ISP1704_OTG_CTRL_IDPULLUP | ISP1704_OTG_CTRL_DP_PULLDOWN;
- musb_ulpi_writeb(musb->mregs, ISP1704_OTG_CTRL, r);
-
+ if(host_mode(musb->mregs)) {
+ musb_force_term(musb->mregs,MUSB_TERM_HOST_FULLSPEED);
+ r = musb_ulpi_readb(musb->mregs,ISP1704_FUNC_CTRL);
+ } else {
+ /* Recover OTG control */
+ r = musb_ulpi_readb(musb->mregs, ISP1704_OTG_CTRL);
+ r |= ISP1704_OTG_CTRL_IDPULLUP | ISP1704_OTG_CTRL_DP_PULLDOWN;
+ musb_ulpi_writeb(musb->mregs, ISP1704_OTG_CTRL, r);
+ r = ISP1704_FUNC_CTRL_FULL_SPEED;
+ }