diff --git a/README.md b/README.md new file mode 100644 index 0000000..ffe2993 --- /dev/null +++ b/README.md @@ -0,0 +1,70 @@ +# Quectel EG25-G GSM Modem + +## Kernel Changes +This package requires the Spiri Mu kernel and modifications to the following files: +* drivers/usb/serial/option.c +* drivers/usb/serial/usb_wwan.c +* drivers/net/usb/qmi_wwan.c +* drivers/net/usb/Makefile + +It also requires these new files: +* drivers/net/usb/GobiUSBNet.c +* drivers/net/usb/QMI.c +* drivers/net/usb/QMI.h +* drivers/net/usb/QMIDevice.c +* drivers/net/usb/QMIDevice.h +* drivers/net/usb/Structs.h + +All these modifications and additions are part of the Spiri Mu kernel. The files are represented in https://git.spirirobotics.com/Spiri/mu_kernel_sources as symlinks from this repository. + +## Installation +Clone the repository: +``` +cd ~/git +git clone https://git.spirirobotics.com/Spiri/mu_packages.git +``` + +Create a udhcpc symbolic link: +``` +cd /bin +sudo ln -s busybox udhcpc +``` + +Create udhcpc directories, copy in the script, and give it execution permission: +``` +sudo mkdir -p /usr/share/udhcpc +sudo mkdir -p /etc/udhcpc/ +sudo cp ~/git/mu_packages/eg25-g/default.script /usr/share/udhcpc +sudo cp ~/git/mu_packages/eg25-g/default.script /etc/udhcpc +sudo chmod +x /usr/share/default.script +sudo chmod +x /etc/udhcpc/default.script +``` + +Build the application: +``` +cd ~/git/mu_packages/eg25-g/quectel-CM +make +``` + +## Running +The following command launches the package, with [apn-name] being substituted with the APN of the wireless provider. For BICS, the APN is bicsapn. To run this in the background, add & to the end of the command. +``` +sudo ./git/mu_packages/eg25-g/quectel-CM/quectel -s [apn-name] +``` + +## Troubleshooting +If the modem is correctly connected to the system, the following command will show the GSM modem attaching to ttyUSB0, ttyUSB1, ttyUSB2, and ttyUSB3: +``` +dmesg | grep GSM +``` + +If the modem has successfully connected to a tower and is being served an IP address, then the following command will show an IP address being served: +``` +ifconfig eth1 +``` + +## Documentation +* Cellular modem hardware design +* Cellular modem PPP +* Cellular modem AT commands +* Cellular modem GNSS AT commands diff --git a/drivers/net/usb/GobiUSBNet.c b/drivers/net/usb/GobiUSBNet.c new file mode 100644 index 0000000..009d057 --- /dev/null +++ b/drivers/net/usb/GobiUSBNet.c @@ -0,0 +1,1437 @@ +/*=========================================================================== +FILE: + GobiUSBNet.c + +DESCRIPTION: + Qualcomm USB Network device for Gobi 3000 + +FUNCTIONS: + GobiNetSuspend + GobiNetResume + GobiNetDriverBind + GobiNetDriverUnbind + GobiUSBNetURBCallback + GobiUSBNetTXTimeout + GobiUSBNetAutoPMThread + GobiUSBNetStartXmit + GobiUSBNetOpen + GobiUSBNetStop + GobiUSBNetProbe + GobiUSBNetModInit + GobiUSBNetModExit + +Copyright (c) 2011, Code Aurora Forum. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Code Aurora Forum nor + the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written + permission. + + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +===========================================================================*/ + +//--------------------------------------------------------------------------- +// Include Files +//--------------------------------------------------------------------------- + +#include "Structs.h" +#include "QMIDevice.h" +#include "QMI.h" +#include +#include +#include + +//----------------------------------------------------------------------------- +// Definitions +//----------------------------------------------------------------------------- + +// Version Information +#define DRIVER_VERSION "GobiNet_SR01A02V14" +#define DRIVER_AUTHOR "Qualcomm Innovation Center" +#define DRIVER_DESC "GobiNet" + +// Debug flag +int debug = 0; + +// Allow user interrupts +int interruptible = 1; + +// Number of IP packets which may be queued up for transmit +int txQueueLength = 100; + +// Class should be created during module init, so needs to be global +static struct class * gpClass; + +#ifdef CONFIG_PM +/*=========================================================================== +METHOD: + GobiNetSuspend (Public Method) + +DESCRIPTION: + Stops QMI traffic while device is suspended + +PARAMETERS + pIntf [ I ] - Pointer to interface + powerEvent [ I ] - Power management event + +RETURN VALUE: + int - 0 for success + negative errno for failure +===========================================================================*/ +int GobiNetSuspend( + struct usb_interface * pIntf, + pm_message_t powerEvent ) +{ + struct usbnet * pDev; + sGobiUSBNet * pGobiDev; + + if (pIntf == 0) + { + return -ENOMEM; + } + +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,23 )) + pDev = usb_get_intfdata( pIntf ); +#else + pDev = (struct usbnet *)pIntf->dev.platform_data; +#endif + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get netdevice\n" ); + return -ENXIO; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return -ENXIO; + } + + // Is this autosuspend or system suspend? + // do we allow remote wakeup? +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,33 )) +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) + if (pDev->udev->auto_pm == 0) +#else + if (1) +#endif +#else + if ((powerEvent.event & PM_EVENT_AUTO) == 0) +#endif + { + DBG( "device suspended to power level %d\n", + powerEvent.event ); + GobiSetDownReason( pGobiDev, DRIVER_SUSPENDED ); + } + else + { + DBG( "device autosuspend\n" ); + } + + if (powerEvent.event & PM_EVENT_SUSPEND) + { + // Stop QMI read callbacks + KillRead( pGobiDev ); +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,22 )) + pDev->udev->reset_resume = 0; +#endif + + // Store power state to avoid duplicate resumes + pIntf->dev.power.power_state.event = powerEvent.event; + } + else + { + // Other power modes cause QMI connection to be lost +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,22 )) + pDev->udev->reset_resume = 1; +#endif + } + + // Run usbnet's suspend function + return usbnet_suspend( pIntf, powerEvent ); +} + +/*=========================================================================== +METHOD: + GobiNetResume (Public Method) + +DESCRIPTION: + Resume QMI traffic or recreate QMI device + +PARAMETERS + pIntf [ I ] - Pointer to interface + +RETURN VALUE: + int - 0 for success + negative errno for failure +===========================================================================*/ +int GobiNetResume( struct usb_interface * pIntf ) +{ + struct usbnet * pDev; + sGobiUSBNet * pGobiDev; + int nRet; + int oldPowerState; + + if (pIntf == 0) + { + return -ENOMEM; + } + +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,23 )) + pDev = usb_get_intfdata( pIntf ); +#else + pDev = (struct usbnet *)pIntf->dev.platform_data; +#endif + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get netdevice\n" ); + return -ENXIO; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return -ENXIO; + } + + oldPowerState = pIntf->dev.power.power_state.event; + pIntf->dev.power.power_state.event = PM_EVENT_ON; + DBG( "resuming from power mode %d\n", oldPowerState ); + + if (oldPowerState & PM_EVENT_SUSPEND) + { + // It doesn't matter if this is autoresume or system resume + GobiClearDownReason( pGobiDev, DRIVER_SUSPENDED ); + + nRet = usbnet_resume( pIntf ); + if (nRet != 0) + { + DBG( "usbnet_resume error %d\n", nRet ); + return nRet; + } + + // Restart QMI read callbacks + nRet = StartRead( pGobiDev ); + if (nRet != 0) + { + DBG( "StartRead error %d\n", nRet ); + return nRet; + } + +#ifdef CONFIG_PM + #if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + // Kick Auto PM thread to process any queued URBs + complete( &pGobiDev->mAutoPM.mThreadDoWork ); + #endif +#endif /* CONFIG_PM */ + } + else + { + DBG( "nothing to resume\n" ); + return 0; + } + + return nRet; +} +#endif /* CONFIG_PM */ + +/*=========================================================================== +METHOD: + GobiNetDriverBind (Public Method) + +DESCRIPTION: + Setup in and out pipes + +PARAMETERS + pDev [ I ] - Pointer to usbnet device + pIntf [ I ] - Pointer to interface + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +static int GobiNetDriverBind( + struct usbnet * pDev, + struct usb_interface * pIntf ) +{ + int numEndpoints; + int endpointIndex; + struct usb_host_endpoint * pEndpoint = NULL; + struct usb_host_endpoint * pIn = NULL; + struct usb_host_endpoint * pOut = NULL; + + // Verify one altsetting + if (pIntf->num_altsetting != 1) + { + DBG( "invalid num_altsetting %u\n", pIntf->num_altsetting ); + return -ENODEV; + } + + // Verify correct interface (4 for UC20) + if ( pIntf->cur_altsetting->desc.bInterfaceNumber != 4 ) + { + DBG( "invalid interface %d\n", + pIntf->cur_altsetting->desc.bInterfaceNumber ); + return -ENODEV; + } + + // Collect In and Out endpoints + numEndpoints = pIntf->cur_altsetting->desc.bNumEndpoints; + for (endpointIndex = 0; endpointIndex < numEndpoints; endpointIndex++) + { + pEndpoint = pIntf->cur_altsetting->endpoint + endpointIndex; + if (pEndpoint == NULL) + { + DBG( "invalid endpoint %u\n", endpointIndex ); + return -ENODEV; + } + + if (usb_endpoint_dir_in( &pEndpoint->desc ) == true + && usb_endpoint_xfer_int( &pEndpoint->desc ) == false) + { + pIn = pEndpoint; + } + else if (usb_endpoint_dir_out( &pEndpoint->desc ) == true) + { + pOut = pEndpoint; + } + } + + if (pIn == NULL || pOut == NULL) + { + DBG( "invalid endpoints\n" ); + return -ENODEV; + } + + if (usb_set_interface( pDev->udev, + pIntf->cur_altsetting->desc.bInterfaceNumber, + 0 ) != 0) + { + DBG( "unable to set interface\n" ); + return -ENODEV; + } + + pDev->in = usb_rcvbulkpipe( pDev->udev, + pIn->desc.bEndpointAddress & USB_ENDPOINT_NUMBER_MASK ); + pDev->out = usb_sndbulkpipe( pDev->udev, + pOut->desc.bEndpointAddress & USB_ENDPOINT_NUMBER_MASK ); + +#if 1 //def DATA_MODE_RP + /* make MAC addr easily distinguishable from an IP header */ + if ((pDev->net->dev_addr[0] & 0xd0) == 0x40) { + /*clear this bit wil make usbnet apdater named as usbX(instead if ethX)*/ + pDev->net->dev_addr[0] |= 0x02; /* set local assignment bit */ + pDev->net->dev_addr[0] &= 0xbf; /* clear "IP" bit */ + } +#endif + + DBG( "in %x, out %x\n", + pIn->desc.bEndpointAddress, + pOut->desc.bEndpointAddress ); + + // In later versions of the kernel, usbnet helps with this +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,23 )) + pIntf->dev.platform_data = (void *)pDev; +#endif + + return 0; +} + +/*=========================================================================== +METHOD: + GobiNetDriverUnbind (Public Method) + +DESCRIPTION: + Deregisters QMI device (Registration happened in the probe function) + +PARAMETERS + pDev [ I ] - Pointer to usbnet device + pIntfUnused [ I ] - Pointer to interface + +RETURN VALUE: + None +===========================================================================*/ +static void GobiNetDriverUnbind( + struct usbnet * pDev, + struct usb_interface * pIntf) +{ + sGobiUSBNet * pGobiDev = (sGobiUSBNet *)pDev->data[0]; + + // Should already be down, but just in case... + netif_carrier_off( pDev->net ); + + DeregisterQMIDevice( pGobiDev ); + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION( 2,6,29 )) + kfree( pDev->net->netdev_ops ); + pDev->net->netdev_ops = NULL; +#endif + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,23 )) + pIntf->dev.platform_data = NULL; +#endif + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION( 2,6,19 )) + pIntf->needs_remote_wakeup = 0; +#endif + + if (atomic_dec_and_test(&pGobiDev->refcount)) + kfree( pGobiDev ); + else + DBG("memory leak!\n"); +} + +#if 1 //def DATA_MODE_RP +/*=========================================================================== +METHOD: + GobiNetDriverTxFixup (Public Method) + +DESCRIPTION: + Handling data format mode on transmit path + +PARAMETERS + pDev [ I ] - Pointer to usbnet device + pSKB [ I ] - Pointer to transmit packet buffer + flags [ I ] - os flags + +RETURN VALUE: + None +===========================================================================*/ +struct sk_buff *GobiNetDriverTxFixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags) +{ + sGobiUSBNet * pGobiDev = (sGobiUSBNet *)dev->data[0]; + + if (!pGobiDev->mbRawIPMode) + return skb; + + // Skip Ethernet header from message + if (skb_pull(skb, ETH_HLEN)) { + return skb; + } else { + dev_err(&dev->intf->dev, "Packet Dropped "); + } + + // Filter the packet out, release it + dev_kfree_skb_any(skb); + return NULL; +} + +/*=========================================================================== +METHOD: + GobiNetDriverRxFixup (Public Method) + +DESCRIPTION: + Handling data format mode on receive path + +PARAMETERS + pDev [ I ] - Pointer to usbnet device + pSKB [ I ] - Pointer to received packet buffer + +RETURN VALUE: + None +===========================================================================*/ +static int GobiNetDriverRxFixup(struct usbnet *dev, struct sk_buff *skb) +{ + __be16 proto; + sGobiUSBNet * pGobiDev = (sGobiUSBNet *)dev->data[0]; + + if (!pGobiDev->mbRawIPMode) + return 1; + + /* This check is no longer done by usbnet */ + if (skb->len < dev->net->hard_header_len) + return 0; + + switch (skb->data[0] & 0xf0) { + case 0x40: + proto = htons(ETH_P_IP); + break; + case 0x60: + proto = htons(ETH_P_IPV6); + break; + case 0x00: + if (is_multicast_ether_addr(skb->data)) + return 1; + /* possibly bogus destination - rewrite just in case */ + skb_reset_mac_header(skb); + goto fix_dest; + default: + /* pass along other packets without modifications */ + return 1; + } + if (skb_headroom(skb) < ETH_HLEN && pskb_expand_head(skb, ETH_HLEN, 0, GFP_ATOMIC)) { + DBG("%s: couldn't pskb_expand_head\n", __func__); + return 0; + } + skb_push(skb, ETH_HLEN); + skb_reset_mac_header(skb); + eth_hdr(skb)->h_proto = proto; + memset(eth_hdr(skb)->h_source, 0, ETH_ALEN); +fix_dest: + memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN); + return 1; +} +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) +#ifdef CONFIG_PM +/*=========================================================================== +METHOD: + GobiUSBNetURBCallback (Public Method) + +DESCRIPTION: + Write is complete, cleanup and signal that we're ready for next packet + +PARAMETERS + pURB [ I ] - Pointer to sAutoPM struct + +RETURN VALUE: + None +===========================================================================*/ +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void GobiUSBNetURBCallback( struct urb * pURB ) +#else +void GobiUSBNetURBCallback(struct urb *pURB, struct pt_regs *regs) +#endif +{ + unsigned long activeURBflags; + sAutoPM * pAutoPM = (sAutoPM *)pURB->context; + if (pAutoPM == NULL) + { + // Should never happen + DBG( "bad context\n" ); + return; + } + + if (pURB->status != 0) + { + // Note that in case of an error, the behaviour is no different + DBG( "urb finished with error %d\n", pURB->status ); + } + + // Remove activeURB (memory to be freed later) + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + + // EAGAIN used to signify callback is done + pAutoPM->mpActiveURB = ERR_PTR( -EAGAIN ); + + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + + complete( &pAutoPM->mThreadDoWork ); + + usb_free_urb( pURB ); +} + +/*=========================================================================== +METHOD: + GobiUSBNetTXTimeout (Public Method) + +DESCRIPTION: + Timeout declared by the net driver. Stop all transfers + +PARAMETERS + pNet [ I ] - Pointer to net device + +RETURN VALUE: + None +===========================================================================*/ +void GobiUSBNetTXTimeout( struct net_device * pNet ) +{ + struct sGobiUSBNet * pGobiDev; + sAutoPM * pAutoPM; + sURBList * pURBListEntry; + unsigned long activeURBflags, URBListFlags; + struct usbnet * pDev = netdev_priv( pNet ); + struct urb * pURB; + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get usbnet device\n" ); + return; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return; + } + pAutoPM = &pGobiDev->mAutoPM; + + DBG( "\n" ); + + // Grab a pointer to active URB + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + pURB = pAutoPM->mpActiveURB; + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + // Stop active URB + if (pURB != NULL) + { + usb_kill_urb( pURB ); + } + + // Cleanup URB List + spin_lock_irqsave( &pAutoPM->mURBListLock, URBListFlags ); + + pURBListEntry = pAutoPM->mpURBList; + while (pURBListEntry != NULL) + { + pAutoPM->mpURBList = pAutoPM->mpURBList->mpNext; + atomic_dec( &pAutoPM->mURBListLen ); + usb_free_urb( pURBListEntry->mpURB ); + kfree( pURBListEntry ); + pURBListEntry = pAutoPM->mpURBList; + } + + spin_unlock_irqrestore( &pAutoPM->mURBListLock, URBListFlags ); + + complete( &pAutoPM->mThreadDoWork ); + + return; +} + +/*=========================================================================== +METHOD: + GobiUSBNetAutoPMThread (Public Method) + +DESCRIPTION: + Handle device Auto PM state asynchronously + Handle network packet transmission asynchronously + +PARAMETERS + pData [ I ] - Pointer to sAutoPM struct + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +static int GobiUSBNetAutoPMThread( void * pData ) +{ + unsigned long activeURBflags, URBListFlags; + sURBList * pURBListEntry; + int status; + struct usb_device * pUdev; + sAutoPM * pAutoPM = (sAutoPM *)pData; + struct urb * pURB; + + if (pAutoPM == NULL) + { + DBG( "passed null pointer\n" ); + return -EINVAL; + } + + pUdev = interface_to_usbdev( pAutoPM->mpIntf ); + + DBG( "traffic thread started\n" ); + + while (pAutoPM->mbExit == false) + { + // Wait for someone to poke us + wait_for_completion_interruptible( &pAutoPM->mThreadDoWork ); + + // Time to exit? + if (pAutoPM->mbExit == true) + { + // Stop activeURB + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + pURB = pAutoPM->mpActiveURB; + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + + // EAGAIN used to signify callback is done + if (IS_ERR( pAutoPM->mpActiveURB ) + && PTR_ERR( pAutoPM->mpActiveURB ) == -EAGAIN ) + { + pURB = NULL; + } + + if (pURB != NULL) + { + usb_kill_urb( pURB ); + } + // Will be freed in callback function + + // Cleanup URB List + spin_lock_irqsave( &pAutoPM->mURBListLock, URBListFlags ); + + pURBListEntry = pAutoPM->mpURBList; + while (pURBListEntry != NULL) + { + pAutoPM->mpURBList = pAutoPM->mpURBList->mpNext; + atomic_dec( &pAutoPM->mURBListLen ); + usb_free_urb( pURBListEntry->mpURB ); + kfree( pURBListEntry ); + pURBListEntry = pAutoPM->mpURBList; + } + + spin_unlock_irqrestore( &pAutoPM->mURBListLock, URBListFlags ); + + break; + } + + // Is our URB active? + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + + // EAGAIN used to signify callback is done + if (IS_ERR( pAutoPM->mpActiveURB ) + && PTR_ERR( pAutoPM->mpActiveURB ) == -EAGAIN ) + { + pAutoPM->mpActiveURB = NULL; + + // Restore IRQs so task can sleep + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + + // URB is done, decrement the Auto PM usage count + usb_autopm_put_interface( pAutoPM->mpIntf ); + + // Lock ActiveURB again + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + } + + if (pAutoPM->mpActiveURB != NULL) + { + // There is already a URB active, go back to sleep + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + continue; + } + + // Is there a URB waiting to be submitted? + spin_lock_irqsave( &pAutoPM->mURBListLock, URBListFlags ); + if (pAutoPM->mpURBList == NULL) + { + // No more URBs to submit, go back to sleep + spin_unlock_irqrestore( &pAutoPM->mURBListLock, URBListFlags ); + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + continue; + } + + // Pop an element + pURBListEntry = pAutoPM->mpURBList; + pAutoPM->mpURBList = pAutoPM->mpURBList->mpNext; + atomic_dec( &pAutoPM->mURBListLen ); + spin_unlock_irqrestore( &pAutoPM->mURBListLock, URBListFlags ); + + // Set ActiveURB + pAutoPM->mpActiveURB = pURBListEntry->mpURB; + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + + // Tell autopm core we need device woken up + status = usb_autopm_get_interface( pAutoPM->mpIntf ); + if (status < 0) + { + DBG( "unable to autoresume interface: %d\n", status ); + + // likely caused by device going from autosuspend -> full suspend + if (status == -EPERM) + { +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,33 )) +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) + pUdev->auto_pm = 0; +#else + pUdev = pUdev; +#endif +#endif + GobiNetSuspend( pAutoPM->mpIntf, PMSG_SUSPEND ); + } + + // Add pURBListEntry back onto pAutoPM->mpURBList + spin_lock_irqsave( &pAutoPM->mURBListLock, URBListFlags ); + pURBListEntry->mpNext = pAutoPM->mpURBList; + pAutoPM->mpURBList = pURBListEntry; + atomic_inc( &pAutoPM->mURBListLen ); + spin_unlock_irqrestore( &pAutoPM->mURBListLock, URBListFlags ); + + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + pAutoPM->mpActiveURB = NULL; + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + + // Go back to sleep + continue; + } + + // Submit URB + status = usb_submit_urb( pAutoPM->mpActiveURB, GFP_KERNEL ); + if (status < 0) + { + // Could happen for a number of reasons + DBG( "Failed to submit URB: %d. Packet dropped\n", status ); + spin_lock_irqsave( &pAutoPM->mActiveURBLock, activeURBflags ); + usb_free_urb( pAutoPM->mpActiveURB ); + pAutoPM->mpActiveURB = NULL; + spin_unlock_irqrestore( &pAutoPM->mActiveURBLock, activeURBflags ); + usb_autopm_put_interface( pAutoPM->mpIntf ); + + // Loop again + complete( &pAutoPM->mThreadDoWork ); + } + + kfree( pURBListEntry ); + } + + DBG( "traffic thread exiting\n" ); + pAutoPM->mpThread = NULL; + return 0; +} + +/*=========================================================================== +METHOD: + GobiUSBNetStartXmit (Public Method) + +DESCRIPTION: + Convert sk_buff to usb URB and queue for transmit + +PARAMETERS + pNet [ I ] - Pointer to net device + +RETURN VALUE: + NETDEV_TX_OK on success + NETDEV_TX_BUSY on error +===========================================================================*/ +int GobiUSBNetStartXmit( + struct sk_buff * pSKB, + struct net_device * pNet ) +{ + unsigned long URBListFlags; + struct sGobiUSBNet * pGobiDev; + sAutoPM * pAutoPM; + sURBList * pURBListEntry, ** ppURBListEnd; + void * pURBData; + struct usbnet * pDev = netdev_priv( pNet ); + + //DBG( "\n" ); + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get usbnet device\n" ); + return NETDEV_TX_BUSY; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return NETDEV_TX_BUSY; + } + pAutoPM = &pGobiDev->mAutoPM; + + if( NULL == pSKB ) + { + DBG( "Buffer is NULL \n" ); + return NETDEV_TX_BUSY; + } + + if (GobiTestDownReason( pGobiDev, DRIVER_SUSPENDED ) == true) + { + // Should not happen + DBG( "device is suspended\n" ); + dump_stack(); + return NETDEV_TX_BUSY; + } + + if (GobiTestDownReason( pGobiDev, NO_NDIS_CONNECTION )) + { + //netif_carrier_off( pGobiDev->mpNetDev->net ); + //DBG( "device is disconnected\n" ); + //dump_stack(); + return NETDEV_TX_BUSY; + } + + // Convert the sk_buff into a URB + + // Check if buffer is full + if ( atomic_read( &pAutoPM->mURBListLen ) >= txQueueLength) + { + DBG( "not scheduling request, buffer is full\n" ); + return NETDEV_TX_BUSY; + } + + // Allocate URBListEntry + pURBListEntry = kmalloc( sizeof( sURBList ), GFP_ATOMIC ); + if (pURBListEntry == NULL) + { + DBG( "unable to allocate URBList memory\n" ); + return NETDEV_TX_BUSY; + } + pURBListEntry->mpNext = NULL; + + // Allocate URB + pURBListEntry->mpURB = usb_alloc_urb( 0, GFP_ATOMIC ); + if (pURBListEntry->mpURB == NULL) + { + DBG( "unable to allocate URB\n" ); + // release all memory allocated by now + if (pURBListEntry) + kfree( pURBListEntry ); + return NETDEV_TX_BUSY; + } + +#if 1 //def DATA_MODE_RP + GobiNetDriverTxFixup(pNet, pSKB, GFP_ATOMIC); +#endif + + // Allocate URB transfer_buffer + pURBData = kmalloc( pSKB->len, GFP_ATOMIC ); + if (pURBData == NULL) + { + DBG( "unable to allocate URB data\n" ); + // release all memory allocated by now + if (pURBListEntry) + { + usb_free_urb( pURBListEntry->mpURB ); + kfree( pURBListEntry ); + } + return NETDEV_TX_BUSY; + } + // Fill with SKB's data + memcpy( pURBData, pSKB->data, pSKB->len ); + + usb_fill_bulk_urb( pURBListEntry->mpURB, + pGobiDev->mpNetDev->udev, + pGobiDev->mpNetDev->out, + pURBData, + pSKB->len, + GobiUSBNetURBCallback, + pAutoPM ); + + /* Handle the need to send a zero length packet and release the + * transfer buffer + */ + pURBListEntry->mpURB->transfer_flags |= (URB_ZERO_PACKET | URB_FREE_BUFFER); + + // Aquire lock on URBList + spin_lock_irqsave( &pAutoPM->mURBListLock, URBListFlags ); + + // Add URB to end of list + ppURBListEnd = &pAutoPM->mpURBList; + while ((*ppURBListEnd) != NULL) + { + ppURBListEnd = &(*ppURBListEnd)->mpNext; + } + *ppURBListEnd = pURBListEntry; + atomic_inc( &pAutoPM->mURBListLen ); + + spin_unlock_irqrestore( &pAutoPM->mURBListLock, URBListFlags ); + + complete( &pAutoPM->mThreadDoWork ); + + // Start transfer timer + pNet->trans_start = jiffies; + // Free SKB + if (pSKB) + dev_kfree_skb_any( pSKB ); + + return NETDEV_TX_OK; +} +#endif +static int (*local_usbnet_start_xmit) (struct sk_buff *skb, struct net_device *net); +#endif + +static int GobiUSBNetStartXmit2( struct sk_buff *pSKB, struct net_device *pNet ){ + struct sGobiUSBNet * pGobiDev; + struct usbnet * pDev = netdev_priv( pNet ); + + //DBG( "\n" ); + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get usbnet device\n" ); + return NETDEV_TX_BUSY; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return NETDEV_TX_BUSY; + } + + if( NULL == pSKB ) + { + DBG( "Buffer is NULL \n" ); + return NETDEV_TX_BUSY; + } + + if (GobiTestDownReason( pGobiDev, DRIVER_SUSPENDED ) == true) + { + // Should not happen + DBG( "device is suspended\n" ); + dump_stack(); + return NETDEV_TX_BUSY; + } + + if (GobiTestDownReason( pGobiDev, NO_NDIS_CONNECTION )) + { + //netif_carrier_off( pGobiDev->mpNetDev->net ); + //DBG( "device is disconnected\n" ); + //dump_stack(); + return NETDEV_TX_BUSY; + } + +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + return local_usbnet_start_xmit(pSKB, pNet); +#else + return usbnet_start_xmit(pSKB, pNet); +#endif +} + +/*=========================================================================== +METHOD: + GobiUSBNetOpen (Public Method) + +DESCRIPTION: + Wrapper to usbnet_open, correctly handling autosuspend + Start AutoPM thread (if CONFIG_PM is defined) + +PARAMETERS + pNet [ I ] - Pointer to net device + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int GobiUSBNetOpen( struct net_device * pNet ) +{ + int status = 0; + struct sGobiUSBNet * pGobiDev; + struct usbnet * pDev = netdev_priv( pNet ); + + if (pDev == NULL) + { + DBG( "failed to get usbnet device\n" ); + return -ENXIO; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return -ENXIO; + } + + DBG( "\n" ); + +#ifdef CONFIG_PM + #if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + // Start the AutoPM thread + pGobiDev->mAutoPM.mpIntf = pGobiDev->mpIntf; + pGobiDev->mAutoPM.mbExit = false; + pGobiDev->mAutoPM.mpURBList = NULL; + pGobiDev->mAutoPM.mpActiveURB = NULL; + spin_lock_init( &pGobiDev->mAutoPM.mURBListLock ); + spin_lock_init( &pGobiDev->mAutoPM.mActiveURBLock ); + atomic_set( &pGobiDev->mAutoPM.mURBListLen, 0 ); + init_completion( &pGobiDev->mAutoPM.mThreadDoWork ); + + pGobiDev->mAutoPM.mpThread = kthread_run( GobiUSBNetAutoPMThread, + &pGobiDev->mAutoPM, + "GobiUSBNetAutoPMThread" ); + if (IS_ERR( pGobiDev->mAutoPM.mpThread )) + { + DBG( "AutoPM thread creation error\n" ); + return PTR_ERR( pGobiDev->mAutoPM.mpThread ); + } + #endif +#endif /* CONFIG_PM */ + + // Allow traffic + GobiClearDownReason( pGobiDev, NET_IFACE_STOPPED ); + + // Pass to usbnet_open if defined + if (pGobiDev->mpUSBNetOpen != NULL) + { + status = pGobiDev->mpUSBNetOpen( pNet ); +#ifdef CONFIG_PM + // If usbnet_open was successful enable Auto PM + if (status == 0) + { +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,33 )) + usb_autopm_enable( pGobiDev->mpIntf ); +#else + usb_autopm_put_interface( pGobiDev->mpIntf ); +#endif + } +#endif /* CONFIG_PM */ + } + else + { + DBG( "no USBNetOpen defined\n" ); + } + + return status; +} + +/*=========================================================================== +METHOD: + GobiUSBNetStop (Public Method) + +DESCRIPTION: + Wrapper to usbnet_stop, correctly handling autosuspend + Stop AutoPM thread (if CONFIG_PM is defined) + +PARAMETERS + pNet [ I ] - Pointer to net device + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int GobiUSBNetStop( struct net_device * pNet ) +{ + struct sGobiUSBNet * pGobiDev; + struct usbnet * pDev = netdev_priv( pNet ); + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get netdevice\n" ); + return -ENXIO; + } + + pGobiDev = (sGobiUSBNet *)pDev->data[0]; + if (pGobiDev == NULL) + { + DBG( "failed to get QMIDevice\n" ); + return -ENXIO; + } + + // Stop traffic + GobiSetDownReason( pGobiDev, NET_IFACE_STOPPED ); + +#ifdef CONFIG_PM + #if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + // Tell traffic thread to exit + pGobiDev->mAutoPM.mbExit = true; + complete( &pGobiDev->mAutoPM.mThreadDoWork ); + + // Wait for it to exit + while( pGobiDev->mAutoPM.mpThread != NULL ) + { + msleep( 100 ); + } + DBG( "thread stopped\n" ); + #endif +#endif /* CONFIG_PM */ + + // Pass to usbnet_stop, if defined + if (pGobiDev->mpUSBNetStop != NULL) + { + return pGobiDev->mpUSBNetStop( pNet ); + } + else + { + return 0; + } +} + +/*=========================================================================*/ +// Struct driver_info +/*=========================================================================*/ +static const struct driver_info GobiNetInfo = +{ + .description = "GobiNet Ethernet Device", +#ifdef CONFIG_ANDROID + .flags = FLAG_ETHER | FLAG_POINTTOPOINT, +#else + .flags = FLAG_ETHER, +#endif + .bind = GobiNetDriverBind, + .unbind = GobiNetDriverUnbind, +#if 1 //def DATA_MODE_RP + .rx_fixup = GobiNetDriverRxFixup, + .tx_fixup = GobiNetDriverTxFixup, +#endif + .data = 0, +}; + +/*=========================================================================*/ +// Qualcomm Gobi 3000 VID/PIDs +/*=========================================================================*/ +static const struct usb_device_id GobiVIDPIDTable [] = +{ + // Quectel UC20 + { + USB_DEVICE( 0x05c6, 0x9003 ), + .driver_info = (unsigned long)&GobiNetInfo + }, + // Quectel EC20 + { + USB_DEVICE( 0x05c6, 0x9215 ), + .driver_info = (unsigned long)&GobiNetInfo + }, + // Quectel EC25 + { + USB_DEVICE( 0x2c7c, 0x0125 ), + .driver_info = (unsigned long)&GobiNetInfo + }, + // Quectel EC21 + { + USB_DEVICE( 0x2c7c, 0x0121 ), + .driver_info = (unsigned long)&GobiNetInfo + }, + //Terminating entry + { } +}; + +MODULE_DEVICE_TABLE( usb, GobiVIDPIDTable ); + +/*=========================================================================== +METHOD: + GobiUSBNetProbe (Public Method) + +DESCRIPTION: + Run usbnet_probe + Setup QMI device + +PARAMETERS + pIntf [ I ] - Pointer to interface + pVIDPIDs [ I ] - Pointer to VID/PID table + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int GobiUSBNetProbe( + struct usb_interface * pIntf, + const struct usb_device_id * pVIDPIDs ) +{ + int status; + struct usbnet * pDev; + sGobiUSBNet * pGobiDev; +#if (LINUX_VERSION_CODE >= KERNEL_VERSION( 2,6,29 )) + struct net_device_ops * pNetDevOps; +#endif + + status = usbnet_probe( pIntf, pVIDPIDs ); + if (status < 0) + { + DBG( "usbnet_probe failed %d\n", status ); + return status; + } + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION( 2,6,19 )) + pIntf->needs_remote_wakeup = 1; +#endif + +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,23 )) + pDev = usb_get_intfdata( pIntf ); +#else + pDev = (struct usbnet *)pIntf->dev.platform_data; +#endif + + if (pDev == NULL || pDev->net == NULL) + { + DBG( "failed to get netdevice\n" ); + usbnet_disconnect( pIntf ); + return -ENXIO; + } + + pGobiDev = kzalloc( sizeof( sGobiUSBNet ), GFP_KERNEL ); + if (pGobiDev == NULL) + { + DBG( "falied to allocate device buffers" ); + usbnet_disconnect( pIntf ); + return -ENOMEM; + } + + atomic_set(&pGobiDev->refcount, 1); + + pDev->data[0] = (unsigned long)pGobiDev; + + pGobiDev->mpNetDev = pDev; + + // Clearing endpoint halt is a magic handshake that brings + // the device out of low power (airplane) mode + usb_clear_halt( pGobiDev->mpNetDev->udev, pDev->out ); + + // Overload PM related network functions +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + pGobiDev->mpUSBNetOpen = pDev->net->open; + pDev->net->open = GobiUSBNetOpen; + pGobiDev->mpUSBNetStop = pDev->net->stop; + pDev->net->stop = GobiUSBNetStop; +#if defined(CONFIG_PM) && (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) + pDev->net->hard_start_xmit = GobiUSBNetStartXmit; + pDev->net->tx_timeout = GobiUSBNetTXTimeout; +#else //quectel donot send dhcp request before ndis connect for uc20 + local_usbnet_start_xmit = pDev->net->hard_start_xmit; + pDev->net->hard_start_xmit = GobiUSBNetStartXmit2; +#endif +#else + pNetDevOps = kmalloc( sizeof( struct net_device_ops ), GFP_KERNEL ); + if (pNetDevOps == NULL) + { + DBG( "falied to allocate net device ops" ); + usbnet_disconnect( pIntf ); + return -ENOMEM; + } + memcpy( pNetDevOps, pDev->net->netdev_ops, sizeof( struct net_device_ops ) ); + + pGobiDev->mpUSBNetOpen = pNetDevOps->ndo_open; + pNetDevOps->ndo_open = GobiUSBNetOpen; + pGobiDev->mpUSBNetStop = pNetDevOps->ndo_stop; + pNetDevOps->ndo_stop = GobiUSBNetStop; +#if 1 //quectel donot send dhcp request before ndis connect for uc20 + pNetDevOps->ndo_start_xmit = GobiUSBNetStartXmit2; +#else + pNetDevOps->ndo_start_xmit = usbnet_start_xmit; +#endif + pNetDevOps->ndo_tx_timeout = usbnet_tx_timeout; + + pDev->net->netdev_ops = pNetDevOps; +#endif + +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,31 )) + memset( &(pGobiDev->mpNetDev->stats), 0, sizeof( struct net_device_stats ) ); +#else + memset( &(pGobiDev->mpNetDev->net->stats), 0, sizeof( struct net_device_stats ) ); +#endif + + pGobiDev->mpIntf = pIntf; + memset( &(pGobiDev->mMEID), '0', 14 ); + + DBG( "Mac Address:\n" ); + PrintHex( &pGobiDev->mpNetDev->net->dev_addr[0], 6 ); + + pGobiDev->mbQMIValid = false; + memset( &pGobiDev->mQMIDev, 0, sizeof( sQMIDev ) ); + pGobiDev->mQMIDev.mbCdevIsInitialized = false; + + pGobiDev->mQMIDev.mpDevClass = gpClass; + +#ifdef CONFIG_PM + #if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + init_completion( &pGobiDev->mAutoPM.mThreadDoWork ); + #endif +#endif /* CONFIG_PM */ + spin_lock_init( &pGobiDev->mQMIDev.mClientMemLock ); + + // Default to device down + pGobiDev->mDownReason = 0; + +//#if (LINUX_VERSION_CODE < KERNEL_VERSION( 3,11,0 )) + GobiSetDownReason( pGobiDev, NO_NDIS_CONNECTION ); + GobiSetDownReason( pGobiDev, NET_IFACE_STOPPED ); +//#endif + + // Register QMI + status = RegisterQMIDevice( pGobiDev ); + if (status != 0) + { + // usbnet_disconnect() will call GobiNetDriverUnbind() which will call + // DeregisterQMIDevice() to clean up any partially created QMI device + usbnet_disconnect( pIntf ); + return status; + } + + // Success + return 0; +} + +static struct usb_driver GobiNet = +{ + .name = "GobiNet", + .id_table = GobiVIDPIDTable, + .probe = GobiUSBNetProbe, + .disconnect = usbnet_disconnect, +#ifdef CONFIG_PM + .suspend = GobiNetSuspend, + .resume = GobiNetResume, +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) + .supports_autosuspend = true, +#endif +#else + .suspend = NULL, + .resume = NULL, +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) + .supports_autosuspend = false, +#endif +#endif /* CONFIG_PM */ +}; + +/*=========================================================================== +METHOD: + GobiUSBNetModInit (Public Method) + +DESCRIPTION: + Initialize module + Create device class + Register out usb_driver struct + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +static int __init GobiUSBNetModInit( void ) +{ + gpClass = class_create( THIS_MODULE, "GobiQMI" ); + if (IS_ERR( gpClass ) == true) + { + DBG( "error at class_create %ld\n", + PTR_ERR( gpClass ) ); + return -ENOMEM; + } + + // This will be shown whenever driver is loaded + printk( KERN_INFO "%s: %s\n", DRIVER_DESC, DRIVER_VERSION ); + + return usb_register( &GobiNet ); +} +module_init( GobiUSBNetModInit ); + +/*=========================================================================== +METHOD: + GobiUSBNetModExit (Public Method) + +DESCRIPTION: + Deregister module + Destroy device class + +RETURN VALUE: + void +===========================================================================*/ +static void __exit GobiUSBNetModExit( void ) +{ + usb_deregister( &GobiNet ); + + class_destroy( gpClass ); +} +module_exit( GobiUSBNetModExit ); + +MODULE_VERSION( DRIVER_VERSION ); +MODULE_AUTHOR( DRIVER_AUTHOR ); +MODULE_DESCRIPTION( DRIVER_DESC ); +MODULE_LICENSE("Dual BSD/GPL"); + +#ifdef bool +#undef bool +#endif + +module_param( debug, int, S_IRUGO | S_IWUSR ); +MODULE_PARM_DESC( debug, "Debuging enabled or not" ); + +module_param( interruptible, int, S_IRUGO | S_IWUSR ); +MODULE_PARM_DESC( interruptible, "Listen for and return on user interrupt" ); +module_param( txQueueLength, int, S_IRUGO | S_IWUSR ); +MODULE_PARM_DESC( txQueueLength, + "Number of IP packets which may be queued up for transmit" ); + diff --git a/drivers/net/usb/Makefile b/drivers/net/usb/Makefile new file mode 100644 index 0000000..33be8c0 --- /dev/null +++ b/drivers/net/usb/Makefile @@ -0,0 +1,43 @@ +# +# Makefile for USB Network drivers +# + +obj-$(CONFIG_USB_CATC) += catc.o +obj-$(CONFIG_USB_KAWETH) += kaweth.o +obj-$(CONFIG_USB_PEGASUS) += pegasus.o +obj-$(CONFIG_USB_RTL8150) += rtl8150.o +obj-$(CONFIG_USB_RTL8152) += r8152.o +obj-$(CONFIG_USB_HSO) += hso.o +obj-$(CONFIG_USB_LAN78XX) += lan78xx.o +obj-$(CONFIG_USB_NET_AX8817X) += asix.o +asix-y := asix_devices.o asix_common.o ax88172a.o +obj-$(CONFIG_USB_NET_AX88179_178A) += ax88179_178a.o +obj-$(CONFIG_USB_NET_CDCETHER) += cdc_ether.o +obj-$(CONFIG_USB_NET_CDC_EEM) += cdc_eem.o +obj-$(CONFIG_USB_NET_DM9601) += dm9601.o +obj-$(CONFIG_USB_NET_SR9700) += sr9700.o +obj-$(CONFIG_USB_NET_SR9800) += sr9800.o +obj-$(CONFIG_USB_NET_SMSC75XX) += smsc75xx.o +obj-$(CONFIG_USB_NET_SMSC95XX) += smsc95xx.o +obj-$(CONFIG_USB_NET_GL620A) += gl620a.o +obj-$(CONFIG_USB_NET_NET1080) += net1080.o +obj-$(CONFIG_USB_NET_PLUSB) += plusb.o +obj-$(CONFIG_USB_NET_RNDIS_HOST) += rndis_host.o +obj-$(CONFIG_USB_NET_CDC_SUBSET_ENABLE) += cdc_subset.o +obj-$(CONFIG_USB_NET_ZAURUS) += zaurus.o +obj-$(CONFIG_USB_NET_MCS7830) += mcs7830.o +obj-$(CONFIG_USB_USBNET) += usbnet.o +obj-$(CONFIG_USB_NET_INT51X1) += int51x1.o +obj-$(CONFIG_USB_CDC_PHONET) += cdc-phonet.o +obj-$(CONFIG_USB_NET_KALMIA) += kalmia.o +obj-$(CONFIG_USB_IPHETH) += ipheth.o +obj-$(CONFIG_USB_SIERRA_NET) += sierra_net.o +obj-$(CONFIG_USB_NET_CX82310_ETH) += cx82310_eth.o +obj-$(CONFIG_USB_NET_CDC_NCM) += cdc_ncm.o +obj-$(CONFIG_USB_NET_HUAWEI_CDC_NCM) += huawei_cdc_ncm.o +obj-$(CONFIG_USB_VL600) += lg-vl600.o +obj-$(CONFIG_USB_NET_QMI_WWAN) += qmi_wwan.o +obj-$(CONFIG_USB_NET_CDC_MBIM) += cdc_mbim.o +obj-$(CONFIG_USB_NET_CH9200) += ch9200.o +obj-y += GobiNet.o +GobiNet-objs := GobiUSBNet.o QMIDevice.o QMI.o diff --git a/drivers/net/usb/QMI.c b/drivers/net/usb/QMI.c new file mode 100644 index 0000000..910c35d --- /dev/null +++ b/drivers/net/usb/QMI.c @@ -0,0 +1,1343 @@ +/*=========================================================================== +FILE: + QMI.c + +DESCRIPTION: + Qualcomm QMI driver code + +FUNCTIONS: + Generic QMUX functions + ParseQMUX + FillQMUX + + Generic QMI functions + GetTLV + ValidQMIMessage + GetQMIMessageID + + Fill Buffers with QMI requests + QMICTLGetClientIDReq + QMICTLReleaseClientIDReq + QMICTLReadyReq + QMIWDSSetEventReportReq + QMIWDSGetPKGSRVCStatusReq + QMIDMSGetMEIDReq + QMIWDASetDataFormatReq + QMICTLSetDataFormatReq + QMICTLSyncReq + + Parse data from QMI responses + QMICTLGetClientIDResp + QMICTLReleaseClientIDResp + QMIWDSEventResp + QMIDMSGetMEIDResp + QMIWDASetDataFormatResp + QMICTLSyncResp + +Copyright (c) 2011, Code Aurora Forum. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Code Aurora Forum nor + the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written + permission. + + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +===========================================================================*/ + +//--------------------------------------------------------------------------- +// Include Files +//--------------------------------------------------------------------------- +#include +#include +#include "Structs.h" +#include "QMI.h" + +/*=========================================================================*/ +// Get sizes of buffers needed by QMI requests +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + QMUXHeaderSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMUXHeaderSize( void ) +{ + return sizeof( sQMUX ); +} + +/*=========================================================================== +METHOD: + QMICTLGetClientIDReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMICTLGetClientIDReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMICTLGetClientIDReqSize( void ) +{ + return sizeof( sQMUX ) + 10; +} + +/*=========================================================================== +METHOD: + QMICTLReleaseClientIDReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMICTLReleaseClientIDReq + +RETURN VALUE: + u16 - size of header +===========================================================================*/ +u16 QMICTLReleaseClientIDReqSize( void ) +{ + return sizeof( sQMUX ) + 11; +} + +/*=========================================================================== +METHOD: + QMICTLReadyReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMICTLReadyReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMICTLReadyReqSize( void ) +{ + return sizeof( sQMUX ) + 6; +} + +/*=========================================================================== +METHOD: + QMIWDSSetEventReportReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMIWDSSetEventReportReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMIWDSSetEventReportReqSize( void ) +{ + return sizeof( sQMUX ) + 15; +} + +/*=========================================================================== +METHOD: + QMIWDSGetPKGSRVCStatusReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMIWDSGetPKGSRVCStatusReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMIWDSGetPKGSRVCStatusReqSize( void ) +{ + return sizeof( sQMUX ) + 7; +} + +/*=========================================================================== +METHOD: + QMIDMSGetMEIDReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMIDMSGetMEIDReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMIDMSGetMEIDReqSize( void ) +{ + return sizeof( sQMUX ) + 7; +} + +/*=========================================================================== +METHOD: + QMIWDASetDataFormatReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMIWDASetDataFormatReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMIWDASetDataFormatReqSize( void ) +{ + return sizeof( sQMUX ) + 25; +} + +/*=========================================================================== +METHOD: + QMICTLSetDataFormatReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMICTLSetDataFormatReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMICTLSetDataFormatReqSize( void ) +{ + return sizeof( sQMUX ) + 15; +} + +/*=========================================================================== +METHOD: + QMICTLSyncReqSize (Public Method) + +DESCRIPTION: + Get size of buffer needed for QMUX + QMICTLSyncReq + +RETURN VALUE: + u16 - size of buffer +===========================================================================*/ +u16 QMICTLSyncReqSize( void ) +{ + return sizeof( sQMUX ) + 6; +} + +/*=========================================================================*/ +// Generic QMUX functions +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + ParseQMUX (Public Method) + +DESCRIPTION: + Remove QMUX headers from a buffer + +PARAMETERS + pClientID [ O ] - On success, will point to Client ID + pBuffer [ I ] - Full Message passed in + buffSize [ I ] - Size of pBuffer + +RETURN VALUE: + int - Positive for size of QMUX header + Negative errno for error +===========================================================================*/ +int ParseQMUX( + u16 * pClientID, + void * pBuffer, + u16 buffSize ) +{ + sQMUX * pQMUXHeader; + + if (pBuffer == 0 || buffSize < 12) + { + return -ENOMEM; + } + + // QMUX Header + pQMUXHeader = (sQMUX *)pBuffer; + + if (pQMUXHeader->mTF != 1 + || le16_to_cpu(get_unaligned(&pQMUXHeader->mLength)) != buffSize - 1 + || pQMUXHeader->mCtrlFlag != 0x80 ) + { + return -EINVAL; + } + + // Client ID + *pClientID = (pQMUXHeader->mQMIClientID << 8) + pQMUXHeader->mQMIService; + + return sizeof( sQMUX ); +} + +/*=========================================================================== +METHOD: + FillQMUX (Public Method) + +DESCRIPTION: + Fill buffer with QMUX headers + +PARAMETERS + clientID [ I ] - Client ID + pBuffer [ O ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer (must be at least 6) + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int FillQMUX( + u16 clientID, + void * pBuffer, + u16 buffSize ) +{ + sQMUX * pQMUXHeader; + + if (pBuffer == 0 || buffSize < sizeof( sQMUX )) + { + return -ENOMEM; + } + + // QMUX Header + pQMUXHeader = (sQMUX *)pBuffer; + + pQMUXHeader->mTF = 1; + put_unaligned(cpu_to_le16(buffSize - 1), &pQMUXHeader->mLength); + //DBG("pQMUXHeader->mLength = 0x%x, buffSize - 1 = 0x%x\n",pQMUXHeader->mLength, buffSize - 1); + pQMUXHeader->mCtrlFlag = 0; + + // Service and Client ID + pQMUXHeader->mQMIService = clientID & 0xff; + pQMUXHeader->mQMIClientID = clientID >> 8; + + return 0; +} + +/*=========================================================================*/ +// Generic QMI functions +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + GetTLV (Public Method) + +DESCRIPTION: + Get data buffer of a specified TLV from a QMI message + + QMI Message shall NOT include SDU + +PARAMETERS + pQMIMessage [ I ] - QMI Message buffer + messageLen [ I ] - Size of QMI Message buffer + type [ I ] - Desired Type + pOutDataBuf [ O ] - Buffer to be filled with TLV + messageLen [ I ] - Size of QMI Message buffer + +RETURN VALUE: + u16 - Size of TLV for success + Negative errno for error +===========================================================================*/ +int GetTLV( + void * pQMIMessage, + u16 messageLen, + u8 type, + void * pOutDataBuf, + u16 bufferLen ) +{ + u16 pos; + u16 tlvSize = 0; + u16 cpyCount; + + if (pQMIMessage == 0 || pOutDataBuf == 0) + { + return -ENOMEM; + } + + for (pos = 4; + pos + 3 < messageLen; + pos += tlvSize + 3) + { + tlvSize = le16_to_cpu( get_unaligned(((u16 *)(pQMIMessage + pos + 1) )) ); + if (*(u8 *)(pQMIMessage + pos) == type) + { + if (bufferLen < tlvSize) + { + return -ENOMEM; + } + + for (cpyCount = 0; cpyCount < tlvSize; cpyCount++) + { + *((char*)(pOutDataBuf + cpyCount)) = *((char*)(pQMIMessage + pos + 3 + cpyCount)); + } + + return tlvSize; + } + } + + return -ENOMSG; +} + +/*=========================================================================== +METHOD: + ValidQMIMessage (Public Method) + +DESCRIPTION: + Check mandatory TLV in a QMI message + + QMI Message shall NOT include SDU + +PARAMETERS + pQMIMessage [ I ] - QMI Message buffer + messageLen [ I ] - Size of QMI Message buffer + +RETURN VALUE: + int - 0 for success (no error) + Negative errno for error + Positive for QMI error code +===========================================================================*/ +int ValidQMIMessage( + void * pQMIMessage, + u16 messageLen ) +{ + char mandTLV[4]; + + if (GetTLV( pQMIMessage, messageLen, 2, &mandTLV[0], 4 ) == 4) + { + // Found TLV + if (*(u16 *)&mandTLV[0] != 0) + { + return le16_to_cpu( get_unaligned(&mandTLV[2]) ); + } + else + { + return 0; + } + } + else + { + return -ENOMSG; + } +} + +/*=========================================================================== +METHOD: + GetQMIMessageID (Public Method) + +DESCRIPTION: + Get the message ID of a QMI message + + QMI Message shall NOT include SDU + +PARAMETERS + pQMIMessage [ I ] - QMI Message buffer + messageLen [ I ] - Size of QMI Message buffer + +RETURN VALUE: + int - Positive for message ID + Negative errno for error +===========================================================================*/ +int GetQMIMessageID( + void * pQMIMessage, + u16 messageLen ) +{ + if (messageLen < 2) + { + return -ENODATA; + } + else + { + return le16_to_cpu( get_unaligned((u16 *)pQMIMessage) ); + } +} + +/*=========================================================================*/ +// Fill Buffers with QMI requests +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + QMICTLGetClientIDReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI CTL Get Client ID Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + serviceType [ I ] - Service type requested + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMICTLGetClientIDReq( + void * pBuffer, + u16 buffSize, + u8 transactionID, + u8 serviceType ) +{ + if (pBuffer == 0 || buffSize < QMICTLGetClientIDReqSize() ) + { + return -ENOMEM; + } + + // QMI CTL GET CLIENT ID + // Request + *(u8 *)(pBuffer + sizeof( sQMUX ))= 0x00; + // Transaction ID + *(u8 *)(pBuffer + sizeof( sQMUX ) + 1) = transactionID; + // Message ID + put_unaligned(cpu_to_le16(0x0022), (u16 *)(pBuffer + sizeof( sQMUX ) + 2)); + // Size of TLV's + put_unaligned(cpu_to_le16(0x0004), (u16 *)(pBuffer + sizeof( sQMUX ) + 4)); + // QMI Service Type + *(u8 *)(pBuffer + sizeof( sQMUX ) + 6) = 0x01; + // Size + put_unaligned(cpu_to_le16(0x0001), (u16 *)(pBuffer + sizeof( sQMUX ) + 7)); + // QMI svc type + *(u8 *)(pBuffer + sizeof( sQMUX ) + 9) = serviceType; + + // success + return sizeof( sQMUX ) + 10; +} + +/*=========================================================================== +METHOD: + QMICTLReleaseClientIDReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI CTL Release Client ID Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + clientID [ I ] - Service type requested + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMICTLReleaseClientIDReq( + void * pBuffer, + u16 buffSize, + u8 transactionID, + u16 clientID ) +{ + if (pBuffer == 0 || buffSize < QMICTLReleaseClientIDReqSize() ) + { + return -ENOMEM; + } + + DBG( "buffSize: 0x%x, transactionID: 0x%x, clientID: 0x%x,\n", + buffSize, transactionID, clientID ); + + // QMI CTL RELEASE CLIENT ID REQ + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + // Transaction ID + *(u8 *)(pBuffer + sizeof( sQMUX ) + 1 ) = transactionID; + // Message ID + put_unaligned( cpu_to_le16(0x0023), (u16 *)(pBuffer + sizeof( sQMUX ) + 2) ); + // Size of TLV's + put_unaligned( cpu_to_le16(0x0005), (u16 *)(pBuffer + sizeof( sQMUX ) + 4) ); + // Release client ID + *(u8 *)(pBuffer + sizeof( sQMUX ) + 6) = 0x01; + // Size + put_unaligned( cpu_to_le16(0x0002), (u16 *)(pBuffer + sizeof( sQMUX ) + 7)); + // QMI svs type / Client ID + put_unaligned(cpu_to_le16(clientID), (u16 *)(pBuffer + sizeof( sQMUX ) + 9)); + + // success + return sizeof( sQMUX ) + 11; +} + +/*=========================================================================== +METHOD: + QMICTLReadyReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI CTL Get Version Info Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMICTLReadyReq( + void * pBuffer, + u16 buffSize, + u8 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMICTLReadyReqSize() ) + { + return -ENOMEM; + } + + DBG("buffSize: 0x%x, transactionID: 0x%x\n", buffSize, transactionID); + + // QMI CTL GET VERSION INFO REQ + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + // Transaction ID + *(u8 *)(pBuffer + sizeof( sQMUX ) + 1) = transactionID; + // Message ID + put_unaligned( cpu_to_le16(0x0021), (u16 *)(pBuffer + sizeof( sQMUX ) + 2) ); + // Size of TLV's + put_unaligned( cpu_to_le16(0x0000), (u16 *)(pBuffer + sizeof( sQMUX ) + 4) ); + + // success + return sizeof( sQMUX ) + 6; +} + +/*=========================================================================== +METHOD: + QMIWDSSetEventReportReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI WDS Set Event Report Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMIWDSSetEventReportReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMIWDSSetEventReportReqSize() ) + { + return -ENOMEM; + } + + // QMI WDS SET EVENT REPORT REQ + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + // Transaction ID + put_unaligned( cpu_to_le16(transactionID), (u16 *)(pBuffer + sizeof( sQMUX ) + 1)); + // Message ID + put_unaligned( cpu_to_le16(0x0001), (u16 *)(pBuffer + sizeof( sQMUX ) + 3)); + // Size of TLV's + put_unaligned(cpu_to_le16(0x0008), (u16 *)(pBuffer + sizeof( sQMUX ) + 5)); + // Report channel rate TLV + *(u8 *)(pBuffer + sizeof( sQMUX ) + 7) = 0x11; + // Size + put_unaligned( cpu_to_le16(0x0005), (u16 *)(pBuffer + sizeof( sQMUX ) + 8)); + // Stats period + *(u8 *)(pBuffer + sizeof( sQMUX ) + 10) = 0x01; + // Stats mask + put_unaligned( cpu_to_le32(0x000000ff), (u32 *)(pBuffer + sizeof( sQMUX ) + 11) ); + + // success + return sizeof( sQMUX ) + 15; +} + +/*=========================================================================== +METHOD: + QMIWDSGetPKGSRVCStatusReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI WDS Get PKG SRVC Status Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMIWDSGetPKGSRVCStatusReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMIWDSGetPKGSRVCStatusReqSize() ) + { + return -ENOMEM; + } + + // QMI WDS Get PKG SRVC Status REQ + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + // Transaction ID + put_unaligned(cpu_to_le16(transactionID), (u16 *)(pBuffer + sizeof( sQMUX ) + 1)); + // Message ID + put_unaligned(cpu_to_le16(0x0022), (u16 *)(pBuffer + sizeof( sQMUX ) + 3)); + // Size of TLV's + put_unaligned(cpu_to_le16(0x0000), (u16 *)(pBuffer + sizeof( sQMUX ) + 5)); + + // success + return sizeof( sQMUX ) + 7; +} + +/*=========================================================================== +METHOD: + QMIDMSGetMEIDReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI DMS Get Serial Numbers Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMIDMSGetMEIDReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMIDMSGetMEIDReqSize() ) + { + return -ENOMEM; + } + + // QMI DMS GET SERIAL NUMBERS REQ + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + // Transaction ID + put_unaligned( cpu_to_le16(transactionID), (u16 *)(pBuffer + sizeof( sQMUX ) + 1) ); + // Message ID + put_unaligned( cpu_to_le16(0x0025), (u16 *)(pBuffer + sizeof( sQMUX ) + 3) ); + // Size of TLV's + put_unaligned( cpu_to_le16(0x0000), (u16 *)(pBuffer + sizeof( sQMUX ) + 5)); + + // success + return sizeof( sQMUX ) + 7; +} + +/*=========================================================================== +METHOD: + QMIWDASetDataFormatReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI WDA Set Data Format Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMIWDASetDataFormatReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMIWDASetDataFormatReqSize() ) + { + return -ENOMEM; + } + + // QMI WDA SET DATA FORMAT REQ + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + + // Transaction ID + put_unaligned( cpu_to_le16(transactionID), (u16 *)(pBuffer + sizeof( sQMUX ) + 1) ); + + // Message ID + put_unaligned( cpu_to_le16(0x0020), (u16 *)(pBuffer + sizeof( sQMUX ) + 3) ); + + // Size of TLV's + put_unaligned( cpu_to_le16(0x0012), (u16 *)(pBuffer + sizeof( sQMUX ) + 5)); + + /* TLVType QOS Data Format 1 byte */ + *(u8 *)(pBuffer + sizeof( sQMUX ) + 7) = 0x10; // type data format + + /* TLVLength 2 bytes - see spec */ + put_unaligned( cpu_to_le16(0x0001), (u16 *)(pBuffer + sizeof( sQMUX ) + 8)); + + /* DataFormat: 0-default; 1-QoS hdr present 2 bytes */ +#ifdef QOS_MODE + *(u8 *)(pBuffer + sizeof( sQMUX ) + 10) = 1; /* QOS header */ +#else + *(u8 *)(pBuffer + sizeof( sQMUX ) + 10) = 0; /* no-QOS header */ +#endif + + /* TLVType Link-Layer Protocol (Optional) 1 byte */ + *(u8 *)(pBuffer + sizeof( sQMUX ) + 11) = 0x11; + + /* TLVLength 2 bytes */ + put_unaligned( cpu_to_le16(0x0004), (u16 *)(pBuffer + sizeof( sQMUX ) + 12)); + + /* LinkProt: 0x1 - ETH; 0x2 - rawIP 4 bytes */ +#ifdef DATA_MODE_RP + /* Set RawIP mode */ + put_unaligned( cpu_to_le32(0x00000002), (u32 *)(pBuffer + sizeof( sQMUX ) + 14)); + DBG("Request RawIP Data Format\n"); +#else + /* Set Ethernet mode */ + put_unaligned( cpu_to_le32(0x00000001), (u32 *)(pBuffer + sizeof( sQMUX ) + 14)); + DBG("Request Ethernet Data Format\n"); +#endif + + /* TLVType Uplink Data Aggression Protocol - 1 byte */ + *(u8 *)(pBuffer + sizeof( sQMUX ) + 18) = 0x13; + + /* TLVLength 2 bytes */ + put_unaligned( cpu_to_le16(0x0004), (u16 *)(pBuffer + sizeof( sQMUX ) + 19)); + + /* TLV Data */ + put_unaligned( cpu_to_le32(0x00000000), (u32 *)(pBuffer + sizeof( sQMUX ) + 21)); + + // success + return QMIWDASetDataFormatReqSize(); +} + + + +/*=========================================================================== +METHOD: + QMICTLSetDataFormatReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI CTL Set Data Format Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMICTLSetDataFormatReq( + void * pBuffer, + u16 buffSize, + u8 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMICTLSetDataFormatReqSize() ) + { + return -ENOMEM; + } + + /* QMI CTL Set Data Format Request */ + /* Request */ + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; // QMICTL_FLAG_REQUEST + + /* Transaction ID 1 byte */ + *(u8 *)(pBuffer + sizeof( sQMUX ) + 1) = transactionID; /* 1 byte as in spec */ + + /* QMICTLType 2 bytes */ + put_unaligned( cpu_to_le16(0x0026), (u16 *)(pBuffer + sizeof( sQMUX ) + 2)); + + /* Length 2 bytes of 2 TLVs each - see spec */ + put_unaligned( cpu_to_le16(0x0009), (u16 *)(pBuffer + sizeof( sQMUX ) + 4)); + + /* TLVType Data Format (Mandatory) 1 byte */ + *(u8 *)(pBuffer + sizeof( sQMUX ) + 6) = 0x01; // type data format + + /* TLVLength 2 bytes - see spec */ + put_unaligned( cpu_to_le16(0x0001), (u16 *)(pBuffer + sizeof( sQMUX ) + 7)); + + /* DataFormat: 0-default; 1-QoS hdr present 2 bytes */ +#ifdef QOS_MODE + *(u8 *)(pBuffer + sizeof( sQMUX ) + 9) = 1; /* QOS header */ +#else + *(u8 *)(pBuffer + sizeof( sQMUX ) + 9) = 0; /* no-QOS header */ +#endif + + /* TLVType Link-Layer Protocol (Optional) 1 byte */ + *(u8 *)(pBuffer + sizeof( sQMUX ) + 10) = TLV_TYPE_LINK_PROTO; + + /* TLVLength 2 bytes */ + put_unaligned( cpu_to_le16(0x0002), (u16 *)(pBuffer + sizeof( sQMUX ) + 11)); + + /* LinkProt: 0x1 - ETH; 0x2 - rawIP 2 bytes */ +#ifdef DATA_MODE_RP + /* Set RawIP mode */ + put_unaligned( cpu_to_le16(0x0002), (u16 *)(pBuffer + sizeof( sQMUX ) + 13)); + DBG("Request RawIP Data Format\n"); +#else + /* Set Ethernet mode */ + put_unaligned( cpu_to_le16(0x0001), (u16 *)(pBuffer + sizeof( sQMUX ) + 13)); + DBG("Request Ethernet Data Format\n"); +#endif + + /* success */ + return sizeof( sQMUX ) + 15; + +} + +/*=========================================================================== +METHOD: + QMICTLSyncReq (Public Method) + +DESCRIPTION: + Fill buffer with QMI CTL Sync Request + +PARAMETERS + pBuffer [ 0 ] - Buffer to be filled + buffSize [ I ] - Size of pBuffer + transactionID [ I ] - Transaction ID + +RETURN VALUE: + int - Positive for resulting size of pBuffer + Negative errno for error +===========================================================================*/ +int QMICTLSyncReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ) +{ + if (pBuffer == 0 || buffSize < QMICTLSyncReqSize() ) + { + return -ENOMEM; + } + + // Request + *(u8 *)(pBuffer + sizeof( sQMUX )) = 0x00; + // Transaction ID + *(u8 *)(pBuffer + sizeof( sQMUX ) + 1) = transactionID; + // Message ID + put_unaligned( cpu_to_le16(0x0027), (u16 *)(pBuffer + sizeof( sQMUX ) + 2) ); + // Size of TLV's + put_unaligned( cpu_to_le16(0x0000), (u16 *)(pBuffer + sizeof( sQMUX ) + 4) ); + + // success + return sizeof( sQMUX ) + 6; +} + +/*=========================================================================*/ +// Parse data from QMI responses +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + QMICTLGetClientIDResp (Public Method) + +DESCRIPTION: + Parse the QMI CTL Get Client ID Resp + +PARAMETERS + pBuffer [ I ] - Buffer to be parsed + buffSize [ I ] - Size of pBuffer + pClientID [ 0 ] - Recieved client ID + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int QMICTLGetClientIDResp( + void * pBuffer, + u16 buffSize, + u16 * pClientID ) +{ + int result; + + // Ignore QMUX and SDU + // QMI CTL SDU is 2 bytes, not 3 + u8 offset = sizeof( sQMUX ) + 2; + + if (pBuffer == 0 || buffSize < offset) + { + return -ENOMEM; + } + + pBuffer = pBuffer + offset; + buffSize -= offset; + + result = GetQMIMessageID( pBuffer, buffSize ); + if (result != 0x22) + { + return -EFAULT; + } + + result = ValidQMIMessage( pBuffer, buffSize ); + if (result != 0) + { + return -EFAULT; + } + + result = GetTLV( pBuffer, buffSize, 0x01, pClientID, 2 ); + if (result != 2) + { + return -EFAULT; + } + + return 0; +} + +/*=========================================================================== +METHOD: + QMICTLReleaseClientIDResp (Public Method) + +DESCRIPTION: + Verify the QMI CTL Release Client ID Resp is valid + +PARAMETERS + pBuffer [ I ] - Buffer to be parsed + buffSize [ I ] - Size of pBuffer + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int QMICTLReleaseClientIDResp( + void * pBuffer, + u16 buffSize ) +{ + int result; + + // Ignore QMUX and SDU + // QMI CTL SDU is 2 bytes, not 3 + u8 offset = sizeof( sQMUX ) + 2; + + if (pBuffer == 0 || buffSize < offset) + { + return -ENOMEM; + } + + pBuffer = pBuffer + offset; + buffSize -= offset; + + result = GetQMIMessageID( pBuffer, buffSize ); + if (result != 0x23) + { + return -EFAULT; + } + + result = ValidQMIMessage( pBuffer, buffSize ); + if (result != 0) + { + return -EFAULT; + } + + return 0; +} + +/*=========================================================================== +METHOD: + QMIWDSEventResp (Public Method) + +DESCRIPTION: + Parse the QMI WDS Set Event Report Resp/Indication or + QMI WDS Get PKG SRVC Status Resp/Indication + + Return parameters will only be updated if value was received + +PARAMETERS + pBuffer [ I ] - Buffer to be parsed + buffSize [ I ] - Size of pBuffer + pTXOk [ O ] - Number of transmitted packets without errors + pRXOk [ O ] - Number of recieved packets without errors + pTXErr [ O ] - Number of transmitted packets with framing errors + pRXErr [ O ] - Number of recieved packets with framing errors + pTXOfl [ O ] - Number of transmitted packets dropped due to overflow + pRXOfl [ O ] - Number of recieved packets dropped due to overflow + pTXBytesOk [ O ] - Number of transmitted bytes without errors + pRXBytesOk [ O ] - Number of recieved bytes without errors + pbLinkState [ 0 ] - Is the link active? + pbReconfigure [ 0 ] - Must interface be reconfigured? (reset IP address) + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int QMIWDSEventResp( + void * pBuffer, + u16 buffSize, + u32 * pTXOk, + u32 * pRXOk, + u32 * pTXErr, + u32 * pRXErr, + u32 * pTXOfl, + u32 * pRXOfl, + u64 * pTXBytesOk, + u64 * pRXBytesOk, + bool * pbLinkState, + bool * pbReconfigure ) +{ + int result; + u8 pktStatusRead[2]; + + // Ignore QMUX and SDU + u8 offset = sizeof( sQMUX ) + 3; + + if (pBuffer == 0 + || buffSize < offset + || pTXOk == 0 + || pRXOk == 0 + || pTXErr == 0 + || pRXErr == 0 + || pTXOfl == 0 + || pRXOfl == 0 + || pTXBytesOk == 0 + || pRXBytesOk == 0 + || pbLinkState == 0 + || pbReconfigure == 0 ) + { + return -ENOMEM; + } + + pBuffer = pBuffer + offset; + buffSize -= offset; + + // Note: Indications. No Mandatory TLV required + + result = GetQMIMessageID( pBuffer, buffSize ); + // QMI WDS Set Event Report Resp + if (result == 0x01) + { + // TLV's are not mandatory + GetTLV( pBuffer, buffSize, 0x10, (void*)pTXOk, 4 ); + put_unaligned( le32_to_cpu(*pTXOk), pTXOk); + GetTLV( pBuffer, buffSize, 0x11, (void*)pRXOk, 4 ); + put_unaligned( le32_to_cpu(*pRXOk), pRXOk); + GetTLV( pBuffer, buffSize, 0x12, (void*)pTXErr, 4 ); + put_unaligned( le32_to_cpu(*pTXErr), pTXErr); + GetTLV( pBuffer, buffSize, 0x13, (void*)pRXErr, 4 ); + put_unaligned( le32_to_cpu(*pRXErr), pRXErr); + GetTLV( pBuffer, buffSize, 0x14, (void*)pTXOfl, 4 ); + put_unaligned( le32_to_cpu(*pTXOfl), pTXOfl); + GetTLV( pBuffer, buffSize, 0x15, (void*)pRXOfl, 4 ); + put_unaligned( le32_to_cpu(*pRXOfl), pRXOfl); + GetTLV( pBuffer, buffSize, 0x19, (void*)pTXBytesOk, 8 ); + put_unaligned( le64_to_cpu(*pTXBytesOk), pTXBytesOk); + GetTLV( pBuffer, buffSize, 0x1A, (void*)pRXBytesOk, 8 ); + put_unaligned( le64_to_cpu(*pRXBytesOk), pRXBytesOk); + } + // QMI WDS Get PKG SRVC Status Resp + else if (result == 0x22) + { + result = GetTLV( pBuffer, buffSize, 0x01, &pktStatusRead[0], 2 ); + // 1 or 2 bytes may be received + if (result >= 1) + { + if (pktStatusRead[0] == 0x02) + { + *pbLinkState = true; + } + else + { + *pbLinkState = false; + } + } + if (result == 2) + { + if (pktStatusRead[1] == 0x01) + { + *pbReconfigure = true; + } + else + { + *pbReconfigure = false; + } + } + + if (result < 0) + { + return result; + } + } + else + { + return -EFAULT; + } + + return 0; +} + +/*=========================================================================== +METHOD: + QMIDMSGetMEIDResp (Public Method) + +DESCRIPTION: + Parse the QMI DMS Get Serial Numbers Resp + +PARAMETERS + pBuffer [ I ] - Buffer to be parsed + buffSize [ I ] - Size of pBuffer + pMEID [ O ] - Device MEID + meidSize [ I ] - Size of MEID buffer (at least 14) + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int QMIDMSGetMEIDResp( + void * pBuffer, + u16 buffSize, + char * pMEID, + int meidSize ) +{ + int result; + + // Ignore QMUX and SDU + u8 offset = sizeof( sQMUX ) + 3; + + if (pBuffer == 0 || buffSize < offset || meidSize < 14) + { + return -ENOMEM; + } + + pBuffer = pBuffer + offset; + buffSize -= offset; + + result = GetQMIMessageID( pBuffer, buffSize ); + if (result != 0x25) + { + return -EFAULT; + } + + result = ValidQMIMessage( pBuffer, buffSize ); + if (result != 0) + { + return -EFAULT; + } + + result = GetTLV( pBuffer, buffSize, 0x12, (void*)pMEID, 14 ); + if (result != 14) + { + return -EFAULT; + } + + return 0; +} + +/*=========================================================================== +METHOD: + QMIWDASetDataFormatResp (Public Method) + +DESCRIPTION: + Parse the QMI WDA Set Data Format Response + +PARAMETERS + pBuffer [ I ] - Buffer to be parsed + buffSize [ I ] - Size of pBuffer + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int QMIWDASetDataFormatResp( + void * pBuffer, + u16 buffSize ) +{ + + int result; + + u8 pktLinkProtocol[4]; + + // Ignore QMUX and SDU + // QMI SDU is 3 bytes + u8 offset = sizeof( sQMUX ) + 3; + + if (pBuffer == 0 || buffSize < offset) + { + return -ENOMEM; + } + + pBuffer = pBuffer + offset; + buffSize -= offset; + + result = GetQMIMessageID( pBuffer, buffSize ); + if (result != 0x20) + { + return -EFAULT; + } + + /* Check response message result TLV */ + result = ValidQMIMessage( pBuffer, buffSize ); + if (result != 0) + { + DBG("EFAULT: Data Format Mode Bad Response\n"); +// return -EFAULT; + return 0; + } + + /* Check response message link protocol */ + result = GetTLV( pBuffer, buffSize, 0x11, + &pktLinkProtocol[0], 4); + if (result != 4) + { + DBG("EFAULT: Wrong TLV format\n"); + return 0; + + } + +#ifdef DATA_MODE_RP + if (pktLinkProtocol[0] != 2) + { + DBG("EFAULT: Data Format Cannot be set to RawIP Mode\n"); + return pktLinkProtocol[0]; + } + DBG("Data Format Set to RawIP\n"); +#else + if (pktLinkProtocol[0] != 1) + { + DBG("EFAULT: Data Format Cannot be set to Ethernet Mode\n"); + return pktLinkProtocol[0]; + } + DBG("Data Format Set to Ethernet Mode \n"); +#endif + + return pktLinkProtocol[0]; +} + +/*=========================================================================== +METHOD: + QMICTLSyncResp (Public Method) + +DESCRIPTION: + Validate the QMI CTL Sync Response + +PARAMETERS + pBuffer [ I ] - Buffer to be parsed + buffSize [ I ] - Size of pBuffer + +RETURN VALUE: + int - 0 for success + Negative errno for error +===========================================================================*/ +int QMICTLSyncResp( + void *pBuffer, + u16 buffSize ) +{ + int result; + + // Ignore QMUX (2 bytes for QMI CTL) and SDU + u8 offset = sizeof( sQMUX ) + 2; + + if (pBuffer == 0 || buffSize < offset) + { + return -ENOMEM; + } + + pBuffer = pBuffer + offset; + buffSize -= offset; + + result = GetQMIMessageID( pBuffer, buffSize ); + if (result != 0x27) + { + return -EFAULT; + } + + result = ValidQMIMessage( pBuffer, buffSize ); + + return result; +} diff --git a/drivers/net/usb/QMI.h b/drivers/net/usb/QMI.h new file mode 100644 index 0000000..8888b76 --- /dev/null +++ b/drivers/net/usb/QMI.h @@ -0,0 +1,314 @@ +/*=========================================================================== +FILE: + QMI.h + +DESCRIPTION: + Qualcomm QMI driver header + +FUNCTIONS: + Generic QMUX functions + ParseQMUX + FillQMUX + + Generic QMI functions + GetTLV + ValidQMIMessage + GetQMIMessageID + + Get sizes of buffers needed by QMI requests + QMUXHeaderSize + QMICTLGetClientIDReqSize + QMICTLReleaseClientIDReqSize + QMICTLReadyReqSize + QMIWDSSetEventReportReqSize + QMIWDSGetPKGSRVCStatusReqSize + QMIDMSGetMEIDReqSize + QMICTLSyncReqSize + + Fill Buffers with QMI requests + QMICTLGetClientIDReq + QMICTLReleaseClientIDReq + QMICTLReadyReq + QMIWDSSetEventReportReq + QMIWDSGetPKGSRVCStatusReq + QMIDMSGetMEIDReq + QMICTLSetDataFormatReq + QMICTLSyncReq + + Parse data from QMI responses + QMICTLGetClientIDResp + QMICTLReleaseClientIDResp + QMIWDSEventResp + QMIDMSGetMEIDResp + +Copyright (c) 2011, Code Aurora Forum. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Code Aurora Forum nor + the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written + permission. + + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +===========================================================================*/ + +#pragma once + +/*=========================================================================*/ +// Definitions +/*=========================================================================*/ + +extern int debug; +// DBG macro +#define DBG( format, arg... ) do { \ + if (debug == 1)\ + { \ + printk( KERN_INFO "GobiNet::%s " format, __FUNCTION__, ## arg ); \ + } }while(0) + +#if 0 +#define VDBG( format, arg... ) do { \ + if (debug == 1)\ + { \ + printk( KERN_INFO "GobiNet::%s " format, __FUNCTION__, ## arg ); \ + } } while(0) +#else +#define VDBG( format, arg... ) do { } while(0) +#endif + +// QMI Service Types +#define QMICTL 0 +#define QMIWDS 1 +#define QMIDMS 2 +#define QMIWDA 0x1A + +#define u8 unsigned char +#define u16 unsigned short +#define u32 unsigned int +#define u64 unsigned long long + +#define bool u8 +#define true 1 +#define false 0 + +#define ENOMEM 12 +#define EFAULT 14 +#define EINVAL 22 +#ifndef ENOMSG +#define ENOMSG 42 +#endif +#define ENODATA 61 + +#define TLV_TYPE_LINK_PROTO 0x10 + +/*=========================================================================*/ +// Struct sQMUX +// +// Structure that defines a QMUX header +/*=========================================================================*/ +typedef struct sQMUX +{ + /* T\F, always 1 */ + u8 mTF; + + /* Size of message */ + u16 mLength; + + /* Control flag */ + u8 mCtrlFlag; + + /* Service Type */ + u8 mQMIService; + + /* Client ID */ + u8 mQMIClientID; + +}__attribute__((__packed__)) sQMUX; + +/*=========================================================================*/ +// Generic QMUX functions +/*=========================================================================*/ + +// Remove QMUX headers from a buffer +int ParseQMUX( + u16 * pClientID, + void * pBuffer, + u16 buffSize ); + +// Fill buffer with QMUX headers +int FillQMUX( + u16 clientID, + void * pBuffer, + u16 buffSize ); + +/*=========================================================================*/ +// Generic QMI functions +/*=========================================================================*/ + +// Get data buffer of a specified TLV from a QMI message +int GetTLV( + void * pQMIMessage, + u16 messageLen, + u8 type, + void * pOutDataBuf, + u16 bufferLen ); + +// Check mandatory TLV in a QMI message +int ValidQMIMessage( + void * pQMIMessage, + u16 messageLen ); + +// Get the message ID of a QMI message +int GetQMIMessageID( + void * pQMIMessage, + u16 messageLen ); + +/*=========================================================================*/ +// Get sizes of buffers needed by QMI requests +/*=========================================================================*/ + +// Get size of buffer needed for QMUX +u16 QMUXHeaderSize( void ); + +// Get size of buffer needed for QMUX + QMICTLGetClientIDReq +u16 QMICTLGetClientIDReqSize( void ); + +// Get size of buffer needed for QMUX + QMICTLReleaseClientIDReq +u16 QMICTLReleaseClientIDReqSize( void ); + +// Get size of buffer needed for QMUX + QMICTLReadyReq +u16 QMICTLReadyReqSize( void ); + +// Get size of buffer needed for QMUX + QMIWDSSetEventReportReq +u16 QMIWDSSetEventReportReqSize( void ); + +// Get size of buffer needed for QMUX + QMIWDSGetPKGSRVCStatusReq +u16 QMIWDSGetPKGSRVCStatusReqSize( void ); + +// Get size of buffer needed for QMUX + QMIDMSGetMEIDReq +u16 QMIDMSGetMEIDReqSize( void ); + +// Get size of buffer needed for QMUX + QMIWDASetDataFormatReq +u16 QMIWDASetDataFormatReqSize( void ); + +// Get size of buffer needed for QMUX + QMICTLSyncReq +u16 QMICTLSyncReqSize( void ); + +/*=========================================================================*/ +// Fill Buffers with QMI requests +/*=========================================================================*/ + +// Fill buffer with QMI CTL Get Client ID Request +int QMICTLGetClientIDReq( + void * pBuffer, + u16 buffSize, + u8 transactionID, + u8 serviceType ); + +// Fill buffer with QMI CTL Release Client ID Request +int QMICTLReleaseClientIDReq( + void * pBuffer, + u16 buffSize, + u8 transactionID, + u16 clientID ); + +// Fill buffer with QMI CTL Get Version Info Request +int QMICTLReadyReq( + void * pBuffer, + u16 buffSize, + u8 transactionID ); + +// Fill buffer with QMI WDS Set Event Report Request +int QMIWDSSetEventReportReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ); + +// Fill buffer with QMI WDS Get PKG SRVC Status Request +int QMIWDSGetPKGSRVCStatusReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ); + +// Fill buffer with QMI DMS Get Serial Numbers Request +int QMIDMSGetMEIDReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ); + +// Fill buffer with QMI WDA Set Data Format Request +int QMIWDASetDataFormatReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ); + +int QMICTLSyncReq( + void * pBuffer, + u16 buffSize, + u16 transactionID ); + +/*=========================================================================*/ +// Parse data from QMI responses +/*=========================================================================*/ + +// Parse the QMI CTL Get Client ID Resp +int QMICTLGetClientIDResp( + void * pBuffer, + u16 buffSize, + u16 * pClientID ); + +// Verify the QMI CTL Release Client ID Resp is valid +int QMICTLReleaseClientIDResp( + void * pBuffer, + u16 buffSize ); + +// Parse the QMI WDS Set Event Report Resp/Indication or +// QMI WDS Get PKG SRVC Status Resp/Indication +int QMIWDSEventResp( + void * pBuffer, + u16 buffSize, + u32 * pTXOk, + u32 * pRXOk, + u32 * pTXErr, + u32 * pRXErr, + u32 * pTXOfl, + u32 * pRXOfl, + u64 * pTXBytesOk, + u64 * pRXBytesOk, + bool * pbLinkState, + bool * pbReconfigure ); + +// Parse the QMI DMS Get Serial Numbers Resp +int QMIDMSGetMEIDResp( + void * pBuffer, + u16 buffSize, + char * pMEID, + int meidSize ); + +// Parse the QMI DMS Get Serial Numbers Resp +int QMIWDASetDataFormatResp( + void * pBuffer, + u16 buffSize ); + +// Pasre the QMI CTL Sync Response +int QMICTLSyncResp( + void *pBuffer, + u16 buffSize ); + diff --git a/drivers/net/usb/QMIDevice.c b/drivers/net/usb/QMIDevice.c new file mode 100644 index 0000000..af9c709 --- /dev/null +++ b/drivers/net/usb/QMIDevice.c @@ -0,0 +1,3994 @@ +/*=========================================================================== +FILE: + QMIDevice.c + +DESCRIPTION: + Functions related to the QMI interface device + +FUNCTIONS: + Generic functions + IsDeviceValid + PrintHex + GobiSetDownReason + GobiClearDownReason + GobiTestDownReason + + Driver level asynchronous read functions + ResubmitIntURB + ReadCallback + IntCallback + StartRead + KillRead + + Internal read/write functions + ReadAsync + UpSem + ReadSync + WriteSyncCallback + WriteSync + + Internal memory management functions + GetClientID + ReleaseClientID + FindClientMem + AddToReadMemList + PopFromReadMemList + AddToNotifyList + NotifyAndPopNotifyList + AddToURBList + PopFromURBList + + Internal userspace wrapper functions + UserspaceunlockedIOCTL + + Userspace wrappers + UserspaceOpen + UserspaceIOCTL + UserspaceClose + UserspaceRead + UserspaceWrite + UserspacePoll + + Initializer and destructor + RegisterQMIDevice + DeregisterQMIDevice + + Driver level client management + QMIReady + QMIWDSCallback + SetupQMIWDSCallback + QMIDMSGetMEID + +Copyright (c) 2011, Code Aurora Forum. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Code Aurora Forum nor + the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written + permission. + + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +===========================================================================*/ + +//--------------------------------------------------------------------------- +// Include Files +//--------------------------------------------------------------------------- +#include +#include "QMIDevice.h" +#include + +//----------------------------------------------------------------------------- +// Definitions +//----------------------------------------------------------------------------- + +extern int debug; +extern int interruptible; +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,22 )) +static int s_interval; +#endif + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,14 )) +#include +static char devfs_name[32]; +int device_create(struct class *class, struct device *parent, dev_t devt, const char *fmt, ...) +{ + va_list vargs; + struct class_device *class_dev; + int err; + + va_start(vargs, fmt); + vsnprintf(devfs_name, sizeof(devfs_name), fmt, vargs); + va_end(vargs); + + class_dev = class_device_create(class, devt, parent, "%s", devfs_name); + if (IS_ERR(class_dev)) { + err = PTR_ERR(class_dev); + goto out; + } + + err = devfs_mk_cdev(devt, S_IFCHR|S_IRUSR|S_IWUSR|S_IRGRP, devfs_name); + if (err) { + class_device_destroy(class, devt); + goto out; + } + + return 0; + +out: + return err; +} + +void device_destroy(struct class *class, dev_t devt) +{ + class_device_destroy(class, devt); + devfs_remove(devfs_name); +} +#endif + +#ifdef CONFIG_PM +// Prototype to GobiNetSuspend function +int GobiNetSuspend( + struct usb_interface * pIntf, + pm_message_t powerEvent ); +#endif /* CONFIG_PM */ + +// IOCTL to generate a client ID for this service type +#define IOCTL_QMI_GET_SERVICE_FILE 0x8BE0 + 1 + +// IOCTL to get the VIDPID of the device +#define IOCTL_QMI_GET_DEVICE_VIDPID 0x8BE0 + 2 + +// IOCTL to get the MEID of the device +#define IOCTL_QMI_GET_DEVICE_MEID 0x8BE0 + 3 + +#define IOCTL_QMI_RELEASE_SERVICE_FILE_IOCTL (0x8BE0 + 4) + +// CDC GET_ENCAPSULATED_RESPONSE packet +#define CDC_GET_ENCAPSULATED_RESPONSE_LE 0x01A1ll +#define CDC_GET_ENCAPSULATED_RESPONSE_BE 0xA101000000000000ll +/* The following masks filter the common part of the encapsulated response + * packet value for Gobi and QMI devices, ie. ignore usb interface number + */ +#define CDC_RSP_MASK_BE 0xFFFFFFFF00FFFFFFll +#define CDC_RSP_MASK_LE 0xFFFFFFE0FFFFFFFFll + +const int i = 1; +#define is_bigendian() ( (*(char*)&i) == 0 ) +#define CDC_GET_ENCAPSULATED_RESPONSE(pcdcrsp, pmask)\ +{\ + *pcdcrsp = is_bigendian() ? CDC_GET_ENCAPSULATED_RESPONSE_BE \ + : CDC_GET_ENCAPSULATED_RESPONSE_LE ; \ + *pmask = is_bigendian() ? CDC_RSP_MASK_BE \ + : CDC_RSP_MASK_LE; \ +} + +// CDC CONNECTION_SPEED_CHANGE indication packet +#define CDC_CONNECTION_SPEED_CHANGE_LE 0x2AA1ll +#define CDC_CONNECTION_SPEED_CHANGE_BE 0xA12A000000000000ll +/* The following masks filter the common part of the connection speed change + * packet value for Gobi and QMI devices + */ +#define CDC_CONNSPD_MASK_BE 0xFFFFFFFFFFFF7FFFll +#define CDC_CONNSPD_MASK_LE 0XFFF7FFFFFFFFFFFFll +#define CDC_GET_CONNECTION_SPEED_CHANGE(pcdccscp, pmask)\ +{\ + *pcdccscp = is_bigendian() ? CDC_CONNECTION_SPEED_CHANGE_BE \ + : CDC_CONNECTION_SPEED_CHANGE_LE ; \ + *pmask = is_bigendian() ? CDC_CONNSPD_MASK_BE \ + : CDC_CONNSPD_MASK_LE; \ +} + +#define SET_CONTROL_LINE_STATE_REQUEST_TYPE 0x21 +#define SET_CONTROL_LINE_STATE_REQUEST 0x22 +#define CONTROL_DTR 0x01 +#define CONTROL_RTS 0x02 + +/*=========================================================================*/ +// UserspaceQMIFops +// QMI device's userspace file operations +/*=========================================================================*/ +struct file_operations UserspaceQMIFops = +{ + .owner = THIS_MODULE, + .read = UserspaceRead, + .write = UserspaceWrite, +#ifdef CONFIG_COMPAT + .compat_ioctl = UserspaceunlockedIOCTL, +#endif +#if (LINUX_VERSION_CODE >= KERNEL_VERSION( 2,6,36 )) + .unlocked_ioctl = UserspaceunlockedIOCTL, +#else + .ioctl = UserspaceIOCTL, +#endif + .open = UserspaceOpen, + .flush = UserspaceClose, + .poll = UserspacePoll, +}; + +/*=========================================================================*/ +// Generic functions +/*=========================================================================*/ +u8 QMIXactionIDGet( sGobiUSBNet *pDev) +{ + u8 transactionID; + + if( 0 == (transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID)) ) + { + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + } + + return transactionID; +} + +static struct usb_endpoint_descriptor *GetEndpoint( + struct usb_interface *pintf, + int type, + int dir ) +{ + int i; + struct usb_host_interface *iface = pintf->cur_altsetting; + struct usb_endpoint_descriptor *pendp; + + for( i = 0; i < iface->desc.bNumEndpoints; i++) + { + pendp = &iface->endpoint[i].desc; + if( ((pendp->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == dir) + && + (usb_endpoint_type(pendp) == type) ) + { + return pendp; + } + } + + return NULL; +} + +/*=========================================================================== +METHOD: + IsDeviceValid (Public Method) + +DESCRIPTION: + Basic test to see if device memory is valid + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + bool +===========================================================================*/ +bool IsDeviceValid( sGobiUSBNet * pDev ) +{ + if (pDev == NULL) + { + return false; + } + + if (pDev->mbQMIValid == false) + { + return false; + } + + return true; +} + +/*=========================================================================== +METHOD: + PrintHex (Public Method) + +DESCRIPTION: + Print Hex data, for debug purposes + +PARAMETERS: + pBuffer [ I ] - Data buffer + bufSize [ I ] - Size of data buffer + +RETURN VALUE: + None +===========================================================================*/ +void PrintHex( + void * pBuffer, + u16 bufSize ) +{ + char * pPrintBuf; + u16 pos; + int status; + + if (debug != 1) + { + return; + } + + pPrintBuf = kmalloc( bufSize * 3 + 1, GFP_ATOMIC ); + if (pPrintBuf == NULL) + { + DBG( "Unable to allocate buffer\n" ); + return; + } + memset( pPrintBuf, 0 , bufSize * 3 + 1 ); + + for (pos = 0; pos < bufSize; pos++) + { + status = snprintf( (pPrintBuf + (pos * 3)), + 4, + "%02X ", + *(u8 *)(pBuffer + pos) ); + if (status != 3) + { + DBG( "snprintf error %d\n", status ); + kfree( pPrintBuf ); + return; + } + } + + DBG( " : %s\n", pPrintBuf ); + + kfree( pPrintBuf ); + pPrintBuf = NULL; + return; +} + +/*=========================================================================== +METHOD: + GobiSetDownReason (Public Method) + +DESCRIPTION: + Sets mDownReason and turns carrier off + +PARAMETERS + pDev [ I ] - Device specific memory + reason [ I ] - Reason device is down + +RETURN VALUE: + None +===========================================================================*/ +void GobiSetDownReason( + sGobiUSBNet * pDev, + u8 reason ) +{ + set_bit( reason, &pDev->mDownReason ); + DBG("%s reason=%d, mDownReason=%x\n", __func__, reason, (unsigned)pDev->mDownReason); + + netif_carrier_off( pDev->mpNetDev->net ); +} + +/*=========================================================================== +METHOD: + GobiClearDownReason (Public Method) + +DESCRIPTION: + Clear mDownReason and may turn carrier on + +PARAMETERS + pDev [ I ] - Device specific memory + reason [ I ] - Reason device is no longer down + +RETURN VALUE: + None +===========================================================================*/ +void GobiClearDownReason( + sGobiUSBNet * pDev, + u8 reason ) +{ + clear_bit( reason, &pDev->mDownReason ); + + DBG("%s reason=%d, mDownReason=%x\n", __func__, reason, (unsigned)pDev->mDownReason); +#if 0 //(LINUX_VERSION_CODE >= KERNEL_VERSION( 3,11,0 )) + netif_carrier_on( pDev->mpNetDev->net ); +#else + if (pDev->mDownReason == 0) + { + netif_carrier_on( pDev->mpNetDev->net ); + } +#endif +} + +/*=========================================================================== +METHOD: + GobiTestDownReason (Public Method) + +DESCRIPTION: + Test mDownReason and returns whether reason is set + +PARAMETERS + pDev [ I ] - Device specific memory + reason [ I ] - Reason device is down + +RETURN VALUE: + bool +===========================================================================*/ +bool GobiTestDownReason( + sGobiUSBNet * pDev, + u8 reason ) +{ + return test_bit( reason, &pDev->mDownReason ); +} + +/*=========================================================================*/ +// Driver level asynchronous read functions +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + ResubmitIntURB (Public Method) + +DESCRIPTION: + Resubmit interrupt URB, re-using same values + +PARAMETERS + pIntURB [ I ] - Interrupt URB + +RETURN VALUE: + int - 0 for success + negative errno for failure +===========================================================================*/ +int ResubmitIntURB( struct urb * pIntURB ) +{ + int status; + int interval; + + // Sanity test + if ( (pIntURB == NULL) + || (pIntURB->dev == NULL) ) + { + return -EINVAL; + } + + // Interval needs reset after every URB completion +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,22 )) + interval = max((int)(pIntURB->ep->desc.bInterval), + (pIntURB->dev->speed == USB_SPEED_HIGH) ? 7 : 3); +#else + interval = s_interval; +#endif + + // Reschedule interrupt URB + usb_fill_int_urb( pIntURB, + pIntURB->dev, + pIntURB->pipe, + pIntURB->transfer_buffer, + pIntURB->transfer_buffer_length, + pIntURB->complete, + pIntURB->context, + interval ); + status = usb_submit_urb( pIntURB, GFP_ATOMIC ); + if (status != 0) + { + DBG( "Error re-submitting Int URB %d\n", status ); + } + + return status; +} + +/*=========================================================================== +METHOD: + ReadCallback (Public Method) + +DESCRIPTION: + Put the data in storage and notify anyone waiting for data + +PARAMETERS + pReadURB [ I ] - URB this callback is run for + +RETURN VALUE: + None +===========================================================================*/ +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void ReadCallback( struct urb * pReadURB ) +#else +void ReadCallback(struct urb *pReadURB, struct pt_regs *regs) +#endif +{ + int result; + u16 clientID; + sClientMemList * pClientMem; + void * pData; + void * pDataCopy; + u16 dataSize; + sGobiUSBNet * pDev; + unsigned long flags; + u16 transactionID; + + if (pReadURB == NULL) + { + DBG( "bad read URB\n" ); + return; + } + + pDev = pReadURB->context; + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return; + } + + if (pReadURB->status != 0) + { + DBG( "Read status = %d\n", pReadURB->status ); + + // Resubmit the interrupt URB + ResubmitIntURB( pDev->mQMIDev.mpIntURB ); + + return; + } + DBG( "Read %d bytes\n", pReadURB->actual_length ); + + pData = pReadURB->transfer_buffer; + dataSize = pReadURB->actual_length; + + PrintHex( pData, dataSize ); + + result = ParseQMUX( &clientID, + pData, + dataSize ); + if (result < 0) + { + DBG( "Read error parsing QMUX %d\n", result ); + + // Resubmit the interrupt URB + ResubmitIntURB( pDev->mQMIDev.mpIntURB ); + + return; + } + + // Grab transaction ID + + // Data large enough? + if (dataSize < result + 3) + { + DBG( "Data buffer too small to parse\n" ); + + // Resubmit the interrupt URB + ResubmitIntURB( pDev->mQMIDev.mpIntURB ); + + return; + } + + // Transaction ID size is 1 for QMICTL, 2 for others + if (clientID == QMICTL) + { + transactionID = *(u8*)(pData + result + 1); + } + else + { + transactionID = le16_to_cpu( get_unaligned((u16*)(pData + result + 1)) ); + } + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Find memory storage for this service and Client ID + // Not using FindClientMem because it can't handle broadcasts + pClientMem = pDev->mQMIDev.mpClientMemList; + + while (pClientMem != NULL) + { + if (pClientMem->mClientID == clientID + || (pClientMem->mClientID | 0xff00) == clientID) + { + // Make copy of pData + pDataCopy = kmalloc( dataSize, GFP_ATOMIC ); + if (pDataCopy == NULL) + { + DBG( "Error allocating client data memory\n" ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Resubmit the interrupt URB + ResubmitIntURB( pDev->mQMIDev.mpIntURB ); + + return; + } + + memcpy( pDataCopy, pData, dataSize ); + + if (AddToReadMemList( pDev, + pClientMem->mClientID, + transactionID, + pDataCopy, + dataSize ) == false) + { + DBG( "Error allocating pReadMemListEntry " + "read will be discarded\n" ); + kfree( pDataCopy ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Resubmit the interrupt URB + ResubmitIntURB( pDev->mQMIDev.mpIntURB ); + + return; + } + + // Success + VDBG( "Creating new readListEntry for client 0x%04X, TID %x\n", + clientID, + transactionID ); + + // Notify this client data exists + NotifyAndPopNotifyList( pDev, + pClientMem->mClientID, + transactionID ); + + // Possibly notify poll() that data exists + wake_up_interruptible_sync( &pClientMem->mWaitQueue ); + + // Not a broadcast + if (clientID >> 8 != 0xff) + { + break; + } + } + + // Next element + pClientMem = pClientMem->mpNext; + } + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Resubmit the interrupt URB + ResubmitIntURB( pDev->mQMIDev.mpIntURB ); +} + +/*=========================================================================== +METHOD: + IntCallback (Public Method) + +DESCRIPTION: + Data is available, fire off a read URB + +PARAMETERS + pIntURB [ I ] - URB this callback is run for + +RETURN VALUE: + None +===========================================================================*/ +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void IntCallback( struct urb * pIntURB ) +{ +#else +void IntCallback(struct urb *pIntURB, struct pt_regs *regs) +{ +#endif + int status; + u64 CDCEncResp; + u64 CDCEncRespMask; + + sGobiUSBNet * pDev = (sGobiUSBNet *)pIntURB->context; + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return; + } + + // Verify this was a normal interrupt + if (pIntURB->status != 0) + { + DBG( "IntCallback: Int status = %d\n", pIntURB->status ); + + // Ignore EOVERFLOW errors + if (pIntURB->status != -EOVERFLOW) + { + // Read 'thread' dies here + return; + } + } + else + { + //TODO cast transfer_buffer to struct usb_cdc_notification + + // CDC GET_ENCAPSULATED_RESPONSE + CDC_GET_ENCAPSULATED_RESPONSE(&CDCEncResp, &CDCEncRespMask) + + VDBG( "IntCallback: Encapsulated Response = 0x%llx\n", + (*(u64*)pIntURB->transfer_buffer)); + + if ((pIntURB->actual_length == 8) + && ((*(u64*)pIntURB->transfer_buffer & CDCEncRespMask) == CDCEncResp ) ) + + { + // Time to read + usb_fill_control_urb( pDev->mQMIDev.mpReadURB, + pDev->mpNetDev->udev, + usb_rcvctrlpipe( pDev->mpNetDev->udev, 0 ), + (unsigned char *)pDev->mQMIDev.mpReadSetupPacket, + pDev->mQMIDev.mpReadBuffer, + DEFAULT_READ_URB_LENGTH, + ReadCallback, + pDev ); + status = usb_submit_urb( pDev->mQMIDev.mpReadURB, GFP_ATOMIC ); + if (status != 0) + { + DBG( "Error submitting Read URB %d\n", status ); + + // Resubmit the interrupt urb + ResubmitIntURB( pIntURB ); + return; + } + + // Int URB will be resubmitted during ReadCallback + return; + } + // CDC CONNECTION_SPEED_CHANGE + else if ((pIntURB->actual_length == 16) + && (CDC_GET_CONNECTION_SPEED_CHANGE(&CDCEncResp, &CDCEncRespMask)) + && ((*(u64*)pIntURB->transfer_buffer & CDCEncRespMask) == CDCEncResp ) ) + { + DBG( "IntCallback: Connection Speed Change = 0x%llx\n", + (*(u64*)pIntURB->transfer_buffer)); + + // if upstream or downstream is 0, stop traffic. Otherwise resume it + if ((*(u32*)(pIntURB->transfer_buffer + 8) == 0) + || (*(u32*)(pIntURB->transfer_buffer + 12) == 0)) + { + GobiSetDownReason( pDev, CDC_CONNECTION_SPEED ); + DBG( "traffic stopping due to CONNECTION_SPEED_CHANGE\n" ); + } + else + { + GobiClearDownReason( pDev, CDC_CONNECTION_SPEED ); + DBG( "resuming traffic due to CONNECTION_SPEED_CHANGE\n" ); + } + } + else + { + DBG( "ignoring invalid interrupt in packet\n" ); + PrintHex( pIntURB->transfer_buffer, pIntURB->actual_length ); + } + } + + // Resubmit the interrupt urb + ResubmitIntURB( pIntURB ); + + return; +} + +/*=========================================================================== +METHOD: + StartRead (Public Method) + +DESCRIPTION: + Start continuous read "thread" (callback driven) + + Note: In case of error, KillRead() should be run + to remove urbs and clean up memory. + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + int - 0 for success + negative errno for failure +===========================================================================*/ +int StartRead( sGobiUSBNet * pDev ) +{ + int interval; + struct usb_endpoint_descriptor *pendp; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return -ENXIO; + } + + // Allocate URB buffers + pDev->mQMIDev.mpReadURB = usb_alloc_urb( 0, GFP_KERNEL ); + if (pDev->mQMIDev.mpReadURB == NULL) + { + DBG( "Error allocating read urb\n" ); + return -ENOMEM; + } + + pDev->mQMIDev.mpIntURB = usb_alloc_urb( 0, GFP_KERNEL ); + if (pDev->mQMIDev.mpIntURB == NULL) + { + DBG( "Error allocating int urb\n" ); + usb_free_urb( pDev->mQMIDev.mpReadURB ); + pDev->mQMIDev.mpReadURB = NULL; + return -ENOMEM; + } + + // Create data buffers + pDev->mQMIDev.mpReadBuffer = kmalloc( DEFAULT_READ_URB_LENGTH, GFP_KERNEL ); + if (pDev->mQMIDev.mpReadBuffer == NULL) + { + DBG( "Error allocating read buffer\n" ); + usb_free_urb( pDev->mQMIDev.mpIntURB ); + pDev->mQMIDev.mpIntURB = NULL; + usb_free_urb( pDev->mQMIDev.mpReadURB ); + pDev->mQMIDev.mpReadURB = NULL; + return -ENOMEM; + } + + pDev->mQMIDev.mpIntBuffer = kmalloc( 64, GFP_KERNEL ); + if (pDev->mQMIDev.mpIntBuffer == NULL) + { + DBG( "Error allocating int buffer\n" ); + kfree( pDev->mQMIDev.mpReadBuffer ); + pDev->mQMIDev.mpReadBuffer = NULL; + usb_free_urb( pDev->mQMIDev.mpIntURB ); + pDev->mQMIDev.mpIntURB = NULL; + usb_free_urb( pDev->mQMIDev.mpReadURB ); + pDev->mQMIDev.mpReadURB = NULL; + return -ENOMEM; + } + + pDev->mQMIDev.mpReadSetupPacket = kmalloc( sizeof( sURBSetupPacket ), + GFP_KERNEL ); + if (pDev->mQMIDev.mpReadSetupPacket == NULL) + { + DBG( "Error allocating setup packet buffer\n" ); + kfree( pDev->mQMIDev.mpIntBuffer ); + pDev->mQMIDev.mpIntBuffer = NULL; + kfree( pDev->mQMIDev.mpReadBuffer ); + pDev->mQMIDev.mpReadBuffer = NULL; + usb_free_urb( pDev->mQMIDev.mpIntURB ); + pDev->mQMIDev.mpIntURB = NULL; + usb_free_urb( pDev->mQMIDev.mpReadURB ); + pDev->mQMIDev.mpReadURB = NULL; + return -ENOMEM; + } + + // CDC Get Encapsulated Response packet + pDev->mQMIDev.mpReadSetupPacket->mRequestType = 0xA1; + pDev->mQMIDev.mpReadSetupPacket->mRequestCode = 1; + pDev->mQMIDev.mpReadSetupPacket->mValue = 0; + pDev->mQMIDev.mpReadSetupPacket->mIndex = + cpu_to_le16(pDev->mpIntf->cur_altsetting->desc.bInterfaceNumber); /* interface number */ + pDev->mQMIDev.mpReadSetupPacket->mLength = cpu_to_le16(DEFAULT_READ_URB_LENGTH); + + pendp = GetEndpoint(pDev->mpIntf, USB_ENDPOINT_XFER_INT, USB_DIR_IN); + if (pendp == NULL) + { + DBG( "Invalid interrupt endpoint!\n" ); + kfree(pDev->mQMIDev.mpReadSetupPacket); + pDev->mQMIDev.mpReadSetupPacket = NULL; + kfree( pDev->mQMIDev.mpIntBuffer ); + pDev->mQMIDev.mpIntBuffer = NULL; + kfree( pDev->mQMIDev.mpReadBuffer ); + pDev->mQMIDev.mpReadBuffer = NULL; + usb_free_urb( pDev->mQMIDev.mpIntURB ); + pDev->mQMIDev.mpIntURB = NULL; + usb_free_urb( pDev->mQMIDev.mpReadURB ); + pDev->mQMIDev.mpReadURB = NULL; + return -ENXIO; + } + + // Interval needs reset after every URB completion + interval = max((int)(pendp->bInterval), + (pDev->mpNetDev->udev->speed == USB_SPEED_HIGH) ? 7 : 3); +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,22 )) + s_interval = interval; +#endif + + // Schedule interrupt URB + usb_fill_int_urb( pDev->mQMIDev.mpIntURB, + pDev->mpNetDev->udev, + /* QMI interrupt endpoint for the following + * interface configuration: DM, NMEA, MDM, NET + */ + usb_rcvintpipe( pDev->mpNetDev->udev, + pendp->bEndpointAddress), + pDev->mQMIDev.mpIntBuffer, + min((int)le16_to_cpu(pendp->wMaxPacketSize), 64), + IntCallback, + pDev, + interval ); + return usb_submit_urb( pDev->mQMIDev.mpIntURB, GFP_KERNEL ); +} + +/*=========================================================================== +METHOD: + KillRead (Public Method) + +DESCRIPTION: + Kill continuous read "thread" + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + None +===========================================================================*/ +void KillRead( sGobiUSBNet * pDev ) +{ + // Stop reading + if (pDev->mQMIDev.mpReadURB != NULL) + { + DBG( "Killng read URB\n" ); + usb_kill_urb( pDev->mQMIDev.mpReadURB ); + } + + if (pDev->mQMIDev.mpIntURB != NULL) + { + DBG( "Killng int URB\n" ); + usb_kill_urb( pDev->mQMIDev.mpIntURB ); + } + + // Release buffers + kfree( pDev->mQMIDev.mpReadSetupPacket ); + pDev->mQMIDev.mpReadSetupPacket = NULL; + kfree( pDev->mQMIDev.mpReadBuffer ); + pDev->mQMIDev.mpReadBuffer = NULL; + kfree( pDev->mQMIDev.mpIntBuffer ); + pDev->mQMIDev.mpIntBuffer = NULL; + + // Release URB's + usb_free_urb( pDev->mQMIDev.mpReadURB ); + pDev->mQMIDev.mpReadURB = NULL; + usb_free_urb( pDev->mQMIDev.mpIntURB ); + pDev->mQMIDev.mpIntURB = NULL; +} + +/*=========================================================================*/ +// Internal read/write functions +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + ReadAsync (Public Method) + +DESCRIPTION: + Start asynchronous read + NOTE: Reading client's data store, not device + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + transactionID [ I ] - Transaction ID or 0 for any + pCallback [ I ] - Callback to be executed when data is available + pData [ I ] - Data buffer that willl be passed (unmodified) + to callback + +RETURN VALUE: + int - 0 for success + negative errno for failure +===========================================================================*/ +int ReadAsync( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void (*pCallback)(sGobiUSBNet*, u16, void *), + void * pData ) +{ + sClientMemList * pClientMem; + sReadMemList ** ppReadMemList; + + unsigned long flags; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return -ENXIO; + } + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Find memory storage for this client ID + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find matching client ID 0x%04X\n", + clientID ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + return -ENXIO; + } + + ppReadMemList = &(pClientMem->mpList); + + // Does data already exist? + while (*ppReadMemList != NULL) + { + // Is this element our data? + if (transactionID == 0 + || transactionID == (*ppReadMemList)->mTransactionID) + { + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Run our own callback + pCallback( pDev, clientID, pData ); + + return 0; + } + + // Next + ppReadMemList = &(*ppReadMemList)->mpNext; + } + + // Data not found, add ourself to list of waiters + if (AddToNotifyList( pDev, + clientID, + transactionID, + pCallback, + pData ) == false) + { + DBG( "Unable to register for notification\n" ); + } + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Success + return 0; +} + +/*=========================================================================== +METHOD: + UpSem (Public Method) + +DESCRIPTION: + Notification function for synchronous read + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + pData [ I ] - Buffer that holds semaphore to be up()-ed + +RETURN VALUE: + None +===========================================================================*/ +void UpSem( + sGobiUSBNet * pDev, + u16 clientID, + void * pData ) +{ + VDBG( "0x%04X\n", clientID ); + + up( (struct semaphore *)pData ); + return; +} + +/*=========================================================================== +METHOD: + ReadSync (Public Method) + +DESCRIPTION: + Start synchronous read + NOTE: Reading client's data store, not device + +PARAMETERS: + pDev [ I ] - Device specific memory + ppOutBuffer [I/O] - On success, will be filled with a + pointer to read buffer + clientID [ I ] - Requester's client ID + transactionID [ I ] - Transaction ID or 0 for any + +RETURN VALUE: + int - size of data read for success + negative errno for failure +===========================================================================*/ +int ReadSync( + sGobiUSBNet * pDev, + void ** ppOutBuffer, + u16 clientID, + u16 transactionID ) +{ + int result; + sClientMemList * pClientMem; + sNotifyList ** ppNotifyList, * pDelNotifyListEntry; + struct semaphore readSem; + void * pData; + unsigned long flags; + u16 dataSize; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return -ENXIO; + } + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Find memory storage for this Client ID + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find matching client ID 0x%04X\n", + clientID ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + return -ENXIO; + } + + // Note: in cases where read is interrupted, + // this will verify client is still valid + while (PopFromReadMemList( pDev, + clientID, + transactionID, + &pData, + &dataSize ) == false) + { + // Data does not yet exist, wait + sema_init( &readSem, 0 ); + + // Add ourself to list of waiters + if (AddToNotifyList( pDev, + clientID, + transactionID, + UpSem, + &readSem ) == false) + { + DBG( "unable to register for notification\n" ); + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + return -EFAULT; + } + + // End critical section while we block + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Wait for notification + result = down_interruptible( &readSem ); + if (result != 0) + { + DBG( "Interrupted %d\n", result ); + + // readSem will fall out of scope, + // remove from notify list so it's not referenced + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + ppNotifyList = &(pClientMem->mpReadNotifyList); + pDelNotifyListEntry = NULL; + + // Find and delete matching entry + while (*ppNotifyList != NULL) + { + if ((*ppNotifyList)->mpData == &readSem) + { + pDelNotifyListEntry = *ppNotifyList; + *ppNotifyList = (*ppNotifyList)->mpNext; + kfree( pDelNotifyListEntry ); + break; + } + + // Next + ppNotifyList = &(*ppNotifyList)->mpNext; + } + + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + return -EINTR; + } + + // Verify device is still valid + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return -ENXIO; + } + + // Restart critical section and continue loop + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + } + + // End Critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Success + *ppOutBuffer = pData; + + return dataSize; +} + +/*=========================================================================== +METHOD: + WriteSyncCallback (Public Method) + +DESCRIPTION: + Write callback + +PARAMETERS + pWriteURB [ I ] - URB this callback is run for + +RETURN VALUE: + None +===========================================================================*/ +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void WriteSyncCallback( struct urb * pWriteURB ) +#else +void WriteSyncCallback(struct urb *pWriteURB, struct pt_regs *regs) +#endif +{ + if (pWriteURB == NULL) + { + DBG( "null urb\n" ); + return; + } + + DBG( "Write status/size %d/%d\n", + pWriteURB->status, + pWriteURB->actual_length ); + + // Notify that write has completed by up()-ing semeaphore + up( (struct semaphore * )pWriteURB->context ); + + return; +} + +/*=========================================================================== +METHOD: + WriteSync (Public Method) + +DESCRIPTION: + Start synchronous write + +PARAMETERS: + pDev [ I ] - Device specific memory + pWriteBuffer [ I ] - Data to be written + writeBufferSize [ I ] - Size of data to be written + clientID [ I ] - Client ID of requester + +RETURN VALUE: + int - write size (includes QMUX) + negative errno for failure +===========================================================================*/ +int WriteSync( + sGobiUSBNet * pDev, + char * pWriteBuffer, + int writeBufferSize, + u16 clientID ) +{ + int result; + struct semaphore writeSem; + struct urb * pWriteURB; + sURBSetupPacket writeSetup; + unsigned long flags; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return -ENXIO; + } + + pWriteURB = usb_alloc_urb( 0, GFP_KERNEL ); + if (pWriteURB == NULL) + { + DBG( "URB mem error\n" ); + return -ENOMEM; + } + + // Fill writeBuffer with QMUX + result = FillQMUX( clientID, pWriteBuffer, writeBufferSize ); + if (result < 0) + { + usb_free_urb( pWriteURB ); + return result; + } + + // CDC Send Encapsulated Request packet + writeSetup.mRequestType = 0x21; + writeSetup.mRequestCode = 0; + writeSetup.mValue = 0; + writeSetup.mIndex = cpu_to_le16(pDev->mpIntf->cur_altsetting->desc.bInterfaceNumber); + writeSetup.mLength = cpu_to_le16(writeBufferSize); + + // Create URB + usb_fill_control_urb( pWriteURB, + pDev->mpNetDev->udev, + usb_sndctrlpipe( pDev->mpNetDev->udev, 0 ), + (unsigned char *)&writeSetup, + (void*)pWriteBuffer, + writeBufferSize, + NULL, + pDev ); + + DBG( "Actual Write:\n" ); + PrintHex( pWriteBuffer, writeBufferSize ); + + sema_init( &writeSem, 0 ); + + pWriteURB->complete = WriteSyncCallback; + pWriteURB->context = &writeSem; + + // Wake device + result = usb_autopm_get_interface( pDev->mpIntf ); + if (result < 0) + { + DBG( "unable to resume interface: %d\n", result ); + + // Likely caused by device going from autosuspend -> full suspend + if (result == -EPERM) + { +#ifdef CONFIG_PM +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,33 )) +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) + pDev->mpNetDev->udev->auto_pm = 0; +#endif +#endif + GobiNetSuspend( pDev->mpIntf, PMSG_SUSPEND ); +#endif /* CONFIG_PM */ + } + usb_free_urb( pWriteURB ); + + return result; + } + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + if (AddToURBList( pDev, clientID, pWriteURB ) == false) + { + usb_free_urb( pWriteURB ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + usb_autopm_put_interface( pDev->mpIntf ); + return -EINVAL; + } + + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + result = usb_submit_urb( pWriteURB, GFP_KERNEL ); + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + if (result < 0) + { + DBG( "submit URB error %d\n", result ); + + // Get URB back so we can destroy it + if (PopFromURBList( pDev, clientID ) != pWriteURB) + { + // This shouldn't happen + DBG( "Didn't get write URB back\n" ); + } + + usb_free_urb( pWriteURB ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + usb_autopm_put_interface( pDev->mpIntf ); + return result; + } + + // End critical section while we block + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Wait for write to finish + if (interruptible != 0) + { + // Allow user interrupts + result = down_interruptible( &writeSem ); + } + else + { + // Ignore user interrupts + result = 0; + down( &writeSem ); + } + + // Write is done, release device + usb_autopm_put_interface( pDev->mpIntf ); + + // Verify device is still valid + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + + usb_free_urb( pWriteURB ); + return -ENXIO; + } + + // Restart critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Get URB back so we can destroy it + if (PopFromURBList( pDev, clientID ) != pWriteURB) + { + // This shouldn't happen + DBG( "Didn't get write URB back\n" ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + usb_free_urb( pWriteURB ); + return -EINVAL; + } + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + if (result == 0) + { + // Write is finished + if (pWriteURB->status == 0) + { + // Return number of bytes that were supposed to have been written, + // not size of QMI request + result = writeBufferSize; + } + else + { + DBG( "bad status = %d\n", pWriteURB->status ); + + // Return error value + result = pWriteURB->status; + } + } + else + { + // We have been forcibly interrupted + DBG( "Interrupted %d !!!\n", result ); + DBG( "Device may be in bad state and need reset !!!\n" ); + + // URB has not finished + usb_kill_urb( pWriteURB ); + } + + usb_free_urb( pWriteURB ); + + return result; +} + +/*=========================================================================*/ +// Internal memory management functions +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + GetClientID (Public Method) + +DESCRIPTION: + Request a QMI client for the input service type and initialize memory + structure + +PARAMETERS: + pDev [ I ] - Device specific memory + serviceType [ I ] - Desired QMI service type + +RETURN VALUE: + int - Client ID for success (positive) + Negative errno for error +===========================================================================*/ +int GetClientID( + sGobiUSBNet * pDev, + u8 serviceType ) +{ + u16 clientID; + sClientMemList ** ppClientMem; + int result; + void * pWriteBuffer; + u16 writeBufferSize; + void * pReadBuffer; + u16 readBufferSize; + unsigned long flags; + u8 transactionID; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device!\n" ); + return -ENXIO; + } + + // Run QMI request to be asigned a Client ID + if (serviceType != 0) + { + writeBufferSize = QMICTLGetClientIDReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + + /* transactionID cannot be 0 */ + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + if (transactionID == 0) + { + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + } + if (transactionID != 0) + { + result = QMICTLGetClientIDReq( pWriteBuffer, + writeBufferSize, + transactionID, + serviceType ); + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + } + else + { + DBG( "Invalid transaction ID!\n" ); + return EINVAL; + } + + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + QMICTL ); + kfree( pWriteBuffer ); + + if (result < 0) + { + return result; + } + + result = ReadSync( pDev, + &pReadBuffer, + QMICTL, + transactionID ); + if (result < 0) + { + DBG( "bad read data %d\n", result ); + return result; + } + readBufferSize = result; + + result = QMICTLGetClientIDResp( pReadBuffer, + readBufferSize, + &clientID ); + + /* Upon return from QMICTLGetClientIDResp, clientID + * low address contains the Service Number (SN), and + * clientID high address contains Client Number (CN) + * For the ReadCallback to function correctly,we swap + * the SN and CN on a Big Endian architecture. + */ + clientID = le16_to_cpu(clientID); + + kfree( pReadBuffer ); + + if (result < 0) + { + return result; + } + } + else + { + // QMI CTL will always have client ID 0 + clientID = 0; + } + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Verify client is not already allocated + if (FindClientMem( pDev, clientID ) != NULL) + { + DBG( "Client memory already exists\n" ); + + // End Critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + return -ETOOMANYREFS; + } + + // Go to last entry in client mem list + ppClientMem = &pDev->mQMIDev.mpClientMemList; + while (*ppClientMem != NULL) + { + ppClientMem = &(*ppClientMem)->mpNext; + } + + // Create locations for read to place data into + *ppClientMem = kmalloc( sizeof( sClientMemList ), GFP_ATOMIC ); + if (*ppClientMem == NULL) + { + DBG( "Error allocating read list\n" ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + return -ENOMEM; + } + + (*ppClientMem)->mClientID = clientID; + (*ppClientMem)->mpList = NULL; + (*ppClientMem)->mpReadNotifyList = NULL; + (*ppClientMem)->mpURBList = NULL; + (*ppClientMem)->mpNext = NULL; + + // Initialize workqueue for poll() + init_waitqueue_head( &(*ppClientMem)->mWaitQueue ); + + // End Critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + return (int)( (*ppClientMem)->mClientID ); +} + +/*=========================================================================== +METHOD: + ReleaseClientID (Public Method) + +DESCRIPTION: + Release QMI client and free memory + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + +RETURN VALUE: + None +===========================================================================*/ +void ReleaseClientID( + sGobiUSBNet * pDev, + u16 clientID ) +{ + int result; + sClientMemList ** ppDelClientMem; + sClientMemList * pNextClientMem; + struct urb * pDelURB; + void * pDelData; + u16 dataSize; + void * pWriteBuffer; + u16 writeBufferSize; + void * pReadBuffer; + u16 readBufferSize; + unsigned long flags; + u8 transactionID; + + // Is device is still valid? + if (IsDeviceValid( pDev ) == false) + { + DBG( "invalid device\n" ); + return; + } + + DBG( "releasing 0x%04X\n", clientID ); + + // Run QMI ReleaseClientID if this isn't QMICTL + if (clientID != QMICTL) + { + // Note: all errors are non fatal, as we always want to delete + // client memory in latter part of function + + writeBufferSize = QMICTLReleaseClientIDReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + DBG( "memory error\n" ); + } + else + { + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + if (transactionID == 0) + { + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + } + result = QMICTLReleaseClientIDReq( pWriteBuffer, + writeBufferSize, + transactionID, + clientID ); + if (result < 0) + { + kfree( pWriteBuffer ); + DBG( "error %d filling req buffer\n", result ); + } + else + { + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + QMICTL ); + kfree( pWriteBuffer ); + + if (result < 0) + { + DBG( "bad write status %d\n", result ); + } + else + { + result = ReadSync( pDev, + &pReadBuffer, + QMICTL, + transactionID ); + if (result < 0) + { + DBG( "bad read status %d\n", result ); + } + else + { + readBufferSize = result; + + result = QMICTLReleaseClientIDResp( pReadBuffer, + readBufferSize ); + kfree( pReadBuffer ); + + if (result < 0) + { + DBG( "error %d parsing response\n", result ); + } + } + } + } + } + } + + // Cleaning up client memory + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Can't use FindClientMem, I need to keep pointer of previous + ppDelClientMem = &pDev->mQMIDev.mpClientMemList; + while (*ppDelClientMem != NULL) + { + if ((*ppDelClientMem)->mClientID == clientID) + { + pNextClientMem = (*ppDelClientMem)->mpNext; + + // Notify all clients + while (NotifyAndPopNotifyList( pDev, + clientID, + 0 ) == true ); + + // Kill and free all URB's + pDelURB = PopFromURBList( pDev, clientID ); + while (pDelURB != NULL) + { + usb_kill_urb( pDelURB ); + usb_free_urb( pDelURB ); + pDelURB = PopFromURBList( pDev, clientID ); + } + + // Free any unread data + while (PopFromReadMemList( pDev, + clientID, + 0, + &pDelData, + &dataSize ) == true ) + { + kfree( pDelData ); + } + + // Delete client Mem + if (!waitqueue_active( &(*ppDelClientMem)->mWaitQueue)) + kfree( *ppDelClientMem ); + else + DBG("memory leak!\n"); + + // Overwrite the pointer that was to this client mem + *ppDelClientMem = pNextClientMem; + } + else + { + // I now point to (a pointer of ((the node I was at)'s mpNext)) + ppDelClientMem = &(*ppDelClientMem)->mpNext; + } + } + + // End Critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + return; +} + +/*=========================================================================== +METHOD: + FindClientMem (Public Method) + +DESCRIPTION: + Find this client's memory + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + +RETURN VALUE: + sClientMemList - Pointer to requested sClientMemList for success + NULL for error +===========================================================================*/ +sClientMemList * FindClientMem( + sGobiUSBNet * pDev, + u16 clientID ) +{ + sClientMemList * pClientMem; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return NULL; + } + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + pClientMem = pDev->mQMIDev.mpClientMemList; + while (pClientMem != NULL) + { + if (pClientMem->mClientID == clientID) + { + // Success + VDBG("Found client's 0x%x memory\n", clientID); + return pClientMem; + } + + pClientMem = pClientMem->mpNext; + } + + DBG( "Could not find client mem 0x%04X\n", clientID ); + return NULL; +} + +/*=========================================================================== +METHOD: + AddToReadMemList (Public Method) + +DESCRIPTION: + Add Data to this client's ReadMem list + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + transactionID [ I ] - Transaction ID or 0 for any + pData [ I ] - Data to add + dataSize [ I ] - Size of data to add + +RETURN VALUE: + bool +===========================================================================*/ +bool AddToReadMemList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void * pData, + u16 dataSize ) +{ + sClientMemList * pClientMem; + sReadMemList ** ppThisReadMemList; + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + // Get this client's memory location + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", + clientID ); + + return false; + } + + // Go to last ReadMemList entry + ppThisReadMemList = &pClientMem->mpList; + while (*ppThisReadMemList != NULL) + { + ppThisReadMemList = &(*ppThisReadMemList)->mpNext; + } + + *ppThisReadMemList = kmalloc( sizeof( sReadMemList ), GFP_ATOMIC ); + if (*ppThisReadMemList == NULL) + { + DBG( "Mem error\n" ); + + return false; + } + + (*ppThisReadMemList)->mpNext = NULL; + (*ppThisReadMemList)->mpData = pData; + (*ppThisReadMemList)->mDataSize = dataSize; + (*ppThisReadMemList)->mTransactionID = transactionID; + + return true; +} + +/*=========================================================================== +METHOD: + PopFromReadMemList (Public Method) + +DESCRIPTION: + Remove data from this client's ReadMem list if it matches + the specified transaction ID. + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + transactionID [ I ] - Transaction ID or 0 for any + ppData [I/O] - On success, will be filled with a + pointer to read buffer + pDataSize [I/O] - On succces, will be filled with the + read buffer's size + +RETURN VALUE: + bool +===========================================================================*/ +bool PopFromReadMemList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void ** ppData, + u16 * pDataSize ) +{ + sClientMemList * pClientMem; + sReadMemList * pDelReadMemList, ** ppReadMemList; + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + // Get this client's memory location + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", + clientID ); + + return false; + } + + ppReadMemList = &(pClientMem->mpList); + pDelReadMemList = NULL; + + // Find first message that matches this transaction ID + while (*ppReadMemList != NULL) + { + // Do we care about transaction ID? + if (transactionID == 0 + || transactionID == (*ppReadMemList)->mTransactionID ) + { + pDelReadMemList = *ppReadMemList; + VDBG( "*ppReadMemList = 0x%p pDelReadMemList = 0x%p\n", + *ppReadMemList, pDelReadMemList ); + break; + } + + VDBG( "skipping 0x%04X data TID = %x\n", clientID, (*ppReadMemList)->mTransactionID ); + + // Next + ppReadMemList = &(*ppReadMemList)->mpNext; + } + VDBG( "*ppReadMemList = 0x%p pDelReadMemList = 0x%p\n", + *ppReadMemList, pDelReadMemList ); + if (pDelReadMemList != NULL) + { + *ppReadMemList = (*ppReadMemList)->mpNext; + + // Copy to output + *ppData = pDelReadMemList->mpData; + *pDataSize = pDelReadMemList->mDataSize; + VDBG( "*ppData = 0x%p pDataSize = %u\n", + *ppData, *pDataSize ); + + // Free memory + kfree( pDelReadMemList ); + + return true; + } + else + { + DBG( "No read memory to pop, Client 0x%04X, TID = %x\n", + clientID, + transactionID ); + return false; + } +} + +/*=========================================================================== +METHOD: + AddToNotifyList (Public Method) + +DESCRIPTION: + Add Notify entry to this client's notify List + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + transactionID [ I ] - Transaction ID or 0 for any + pNotifyFunct [ I ] - Callback function to be run when data is available + pData [ I ] - Data buffer that willl be passed (unmodified) + to callback + +RETURN VALUE: + bool +===========================================================================*/ +bool AddToNotifyList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void (* pNotifyFunct)(sGobiUSBNet *, u16, void *), + void * pData ) +{ + sClientMemList * pClientMem; + sNotifyList ** ppThisNotifyList; + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + // Get this client's memory location + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", clientID ); + return false; + } + + // Go to last URBList entry + ppThisNotifyList = &pClientMem->mpReadNotifyList; + while (*ppThisNotifyList != NULL) + { + ppThisNotifyList = &(*ppThisNotifyList)->mpNext; + } + + *ppThisNotifyList = kmalloc( sizeof( sNotifyList ), GFP_ATOMIC ); + if (*ppThisNotifyList == NULL) + { + DBG( "Mem error\n" ); + return false; + } + + (*ppThisNotifyList)->mpNext = NULL; + (*ppThisNotifyList)->mpNotifyFunct = pNotifyFunct; + (*ppThisNotifyList)->mpData = pData; + (*ppThisNotifyList)->mTransactionID = transactionID; + + return true; +} + +/*=========================================================================== +METHOD: + NotifyAndPopNotifyList (Public Method) + +DESCRIPTION: + Remove first Notify entry from this client's notify list + and Run function + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + transactionID [ I ] - Transaction ID or 0 for any + +RETURN VALUE: + bool +===========================================================================*/ +bool NotifyAndPopNotifyList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID ) +{ + sClientMemList * pClientMem; + sNotifyList * pDelNotifyList, ** ppNotifyList; + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + // Get this client's memory location + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", clientID ); + return false; + } + + ppNotifyList = &(pClientMem->mpReadNotifyList); + pDelNotifyList = NULL; + + // Remove from list + while (*ppNotifyList != NULL) + { + // Do we care about transaction ID? + if (transactionID == 0 + || (*ppNotifyList)->mTransactionID == 0 + || transactionID == (*ppNotifyList)->mTransactionID) + { + pDelNotifyList = *ppNotifyList; + break; + } + + DBG( "skipping data TID = %x\n", (*ppNotifyList)->mTransactionID ); + + // next + ppNotifyList = &(*ppNotifyList)->mpNext; + } + + if (pDelNotifyList != NULL) + { + // Remove element + *ppNotifyList = (*ppNotifyList)->mpNext; + + // Run notification function + if (pDelNotifyList->mpNotifyFunct != NULL) + { + // Unlock for callback + spin_unlock( &pDev->mQMIDev.mClientMemLock ); + + pDelNotifyList->mpNotifyFunct( pDev, + clientID, + pDelNotifyList->mpData ); + + // Restore lock + spin_lock( &pDev->mQMIDev.mClientMemLock ); + } + + // Delete memory + kfree( pDelNotifyList ); + + return true; + } + else + { + DBG( "no one to notify for TID %x\n", transactionID ); + + return false; + } +} + +/*=========================================================================== +METHOD: + AddToURBList (Public Method) + +DESCRIPTION: + Add URB to this client's URB list + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + pURB [ I ] - URB to be added + +RETURN VALUE: + bool +===========================================================================*/ +bool AddToURBList( + sGobiUSBNet * pDev, + u16 clientID, + struct urb * pURB ) +{ + sClientMemList * pClientMem; + sURBList ** ppThisURBList; + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + // Get this client's memory location + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", clientID ); + return false; + } + + // Go to last URBList entry + ppThisURBList = &pClientMem->mpURBList; + while (*ppThisURBList != NULL) + { + ppThisURBList = &(*ppThisURBList)->mpNext; + } + + *ppThisURBList = kmalloc( sizeof( sURBList ), GFP_ATOMIC ); + if (*ppThisURBList == NULL) + { + DBG( "Mem error\n" ); + return false; + } + + (*ppThisURBList)->mpNext = NULL; + (*ppThisURBList)->mpURB = pURB; + + return true; +} + +/*=========================================================================== +METHOD: + PopFromURBList (Public Method) + +DESCRIPTION: + Remove URB from this client's URB list + + Caller MUST have lock on mClientMemLock + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Requester's client ID + +RETURN VALUE: + struct urb - Pointer to requested client's URB + NULL for error +===========================================================================*/ +struct urb * PopFromURBList( + sGobiUSBNet * pDev, + u16 clientID ) +{ + sClientMemList * pClientMem; + sURBList * pDelURBList; + struct urb * pURB; + +#ifdef CONFIG_SMP + // Verify Lock + if (spin_is_locked( &pDev->mQMIDev.mClientMemLock ) == 0) + { + DBG( "unlocked\n" ); + BUG(); + } +#endif + + // Get this client's memory location + pClientMem = FindClientMem( pDev, clientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", clientID ); + return NULL; + } + + // Remove from list + if (pClientMem->mpURBList != NULL) + { + pDelURBList = pClientMem->mpURBList; + pClientMem->mpURBList = pClientMem->mpURBList->mpNext; + + // Copy to output + pURB = pDelURBList->mpURB; + + // Delete memory + kfree( pDelURBList ); + + return pURB; + } + else + { + DBG( "No URB's to pop\n" ); + + return NULL; + } +} + +#ifndef f_dentry +#define f_dentry f_path.dentry +#endif + +/*=========================================================================*/ +// Internal userspace wrappers +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + UserspaceunlockedIOCTL (Public Method) + +DESCRIPTION: + Internal wrapper for Userspace IOCTL interface + +PARAMETERS + pFilp [ I ] - userspace file descriptor + cmd [ I ] - IOCTL command + arg [ I ] - IOCTL argument + +RETURN VALUE: + long - 0 for success + Negative errno for failure +===========================================================================*/ +long UserspaceunlockedIOCTL( + struct file * pFilp, + unsigned int cmd, + unsigned long arg ) +{ + int result; + u32 devVIDPID; + + sQMIFilpStorage * pFilpData = (sQMIFilpStorage *)pFilp->private_data; + + if (pFilpData == NULL) + { + DBG( "Bad file data\n" ); + return -EBADF; + } + + if (IsDeviceValid( pFilpData->mpDev ) == false) + { + DBG( "Invalid device! Updating f_ops\n" ); + pFilp->f_op = pFilp->f_dentry->d_inode->i_fop; + return -ENXIO; + } + + switch (cmd) + { + case IOCTL_QMI_GET_SERVICE_FILE: + DBG( "Setting up QMI for service %lu\n", arg ); + if ((u8)arg == 0) + { + DBG( "Cannot use QMICTL from userspace\n" ); + return -EINVAL; + } + + // Connection is already setup + if (pFilpData->mClientID != (u16)-1) + { + DBG( "Close the current connection before opening a new one\n" ); + return -EBADR; + } + + result = GetClientID( pFilpData->mpDev, (u8)arg ); +// it seems QMIWDA only allow one client, if the last quectel-CM donot realese it (killed by SIGKILL). +// can force release it at here +#if 1 + if (result < 0 && (u8)arg == QMIWDA) + { + ReleaseClientID( pFilpData->mpDev, QMIWDA | (1 << 8) ); + result = GetClientID( pFilpData->mpDev, (u8)arg ); + } +#endif + if (result < 0) + { + return result; + } + pFilpData->mClientID = (u16)result; + DBG("pFilpData->mClientID = 0x%x\n", pFilpData->mClientID ); + return 0; + break; + + + case IOCTL_QMI_GET_DEVICE_VIDPID: + if (arg == 0) + { + DBG( "Bad VIDPID buffer\n" ); + return -EINVAL; + } + + // Extra verification + if (pFilpData->mpDev->mpNetDev == 0) + { + DBG( "Bad mpNetDev\n" ); + return -ENOMEM; + } + if (pFilpData->mpDev->mpNetDev->udev == 0) + { + DBG( "Bad udev\n" ); + return -ENOMEM; + } + + devVIDPID = ((le16_to_cpu( pFilpData->mpDev->mpNetDev->udev->descriptor.idVendor ) << 16) + + le16_to_cpu( pFilpData->mpDev->mpNetDev->udev->descriptor.idProduct ) ); + + result = copy_to_user( (unsigned int *)arg, &devVIDPID, 4 ); + if (result != 0) + { + DBG( "Copy to userspace failure %d\n", result ); + } + + return result; + + break; + + case IOCTL_QMI_GET_DEVICE_MEID: + if (arg == 0) + { + DBG( "Bad MEID buffer\n" ); + return -EINVAL; + } + + result = copy_to_user( (unsigned int *)arg, &pFilpData->mpDev->mMEID[0], 14 ); + if (result != 0) + { + DBG( "Copy to userspace failure %d\n", result ); + } + + return result; + + break; + + default: + return -EBADRQC; + } +} + +/*=========================================================================*/ +// Userspace wrappers +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + UserspaceOpen (Public Method) + +DESCRIPTION: + Userspace open + IOCTL must be called before reads or writes + +PARAMETERS + pInode [ I ] - kernel file descriptor + pFilp [ I ] - userspace file descriptor + +RETURN VALUE: + int - 0 for success + Negative errno for failure +===========================================================================*/ +int UserspaceOpen( + struct inode * pInode, + struct file * pFilp ) +{ + sQMIFilpStorage * pFilpData; + + // Optain device pointer from pInode + sQMIDev * pQMIDev = container_of( pInode->i_cdev, + sQMIDev, + mCdev ); + sGobiUSBNet * pDev = container_of( pQMIDev, + sGobiUSBNet, + mQMIDev ); + + if (pDev->mpNetDev->udev->descriptor.idVendor == cpu_to_le16(0x2c7c)) + { + atomic_inc(&pDev->refcount); + if (!pDev->mbQMIReady) { + if (wait_for_completion_interruptible_timeout(&pDev->mQMIReadyCompletion, 15*HZ) <= 0) { + if (atomic_dec_and_test(&pDev->refcount)) { + kfree( pDev ); + } + return -ETIMEDOUT; + } + } + atomic_dec(&pDev->refcount); + } + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return -ENXIO; + } + + // Setup data in pFilp->private_data + pFilp->private_data = kmalloc( sizeof( sQMIFilpStorage ), GFP_KERNEL ); + if (pFilp->private_data == NULL) + { + DBG( "Mem error\n" ); + return -ENOMEM; + } + + pFilpData = (sQMIFilpStorage *)pFilp->private_data; + pFilpData->mClientID = (u16)-1; + atomic_inc(&pDev->refcount); + pFilpData->mpDev = pDev; + + return 0; +} + +/*=========================================================================== +METHOD: + UserspaceIOCTL (Public Method) + +DESCRIPTION: + Userspace IOCTL functions + +PARAMETERS + pUnusedInode [ I ] - (unused) kernel file descriptor + pFilp [ I ] - userspace file descriptor + cmd [ I ] - IOCTL command + arg [ I ] - IOCTL argument + +RETURN VALUE: + int - 0 for success + Negative errno for failure +===========================================================================*/ +int UserspaceIOCTL( + struct inode * pUnusedInode, + struct file * pFilp, + unsigned int cmd, + unsigned long arg ) +{ + // call the internal wrapper function + return (int)UserspaceunlockedIOCTL( pFilp, cmd, arg ); +} + +/*=========================================================================== +METHOD: + UserspaceClose (Public Method) + +DESCRIPTION: + Userspace close + Release client ID and free memory + +PARAMETERS + pFilp [ I ] - userspace file descriptor + unusedFileTable [ I ] - (unused) file table + +RETURN VALUE: + int - 0 for success + Negative errno for failure +===========================================================================*/ +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +int UserspaceClose( + struct file * pFilp, + fl_owner_t unusedFileTable ) +#else +int UserspaceClose( struct file * pFilp ) +#endif +{ + sQMIFilpStorage * pFilpData = (sQMIFilpStorage *)pFilp->private_data; + struct task_struct * pEachTask; + struct fdtable * pFDT; + int count = 0; + int used = 0; + unsigned long flags; + + if (pFilpData == NULL) + { + DBG( "bad file data\n" ); + return -EBADF; + } + + // Fallthough. If f_count == 1 no need to do more checks +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,24 )) + if (atomic_read( &pFilp->f_count ) != 1) +#else + if (atomic_long_read( &pFilp->f_count ) != 1) +#endif + { + rcu_read_lock(); + for_each_process( pEachTask ) + { + task_lock(pEachTask); + if (pEachTask == NULL || pEachTask->files == NULL) + { + // Some tasks may not have files (e.g. Xsession) + task_unlock(pEachTask); + continue; + } + spin_lock_irqsave( &pEachTask->files->file_lock, flags ); + task_unlock(pEachTask); //kernel/exit.c:do_exit() -> fs/file.c:exit_files() + pFDT = files_fdtable( pEachTask->files ); + for (count = 0; count < pFDT->max_fds; count++) + { + // Before this function was called, this file was removed + // from our task's file table so if we find it in a file + // table then it is being used by another task + if (pFDT->fd[count] == pFilp) + { + used++; + break; + } + } + spin_unlock_irqrestore( &pEachTask->files->file_lock, flags ); + } + rcu_read_unlock(); + + if (used > 0) + { + DBG( "not closing, as this FD is open by %d other process\n", used ); + return 0; + } + } + + if (IsDeviceValid( pFilpData->mpDev ) == false) + { + DBG( "Invalid device! Updating f_ops\n" ); + pFilp->f_op = pFilp->f_dentry->d_inode->i_fop; + return -ENXIO; + } + + DBG( "0x%04X\n", pFilpData->mClientID ); + + // Disable pFilpData so they can't keep sending read or write + // should this function hang + // Note: memory pointer is still saved in pFilpData to be deleted later + pFilp->private_data = NULL; + + if (pFilpData->mClientID != (u16)-1) + { + if (pFilpData->mpDev->mbDeregisterQMIDevice) + pFilpData->mClientID = -1; //DeregisterQMIDevice() will release this ClientID + else + ReleaseClientID( pFilpData->mpDev, + pFilpData->mClientID ); + } + atomic_dec(&pFilpData->mpDev->refcount); + + kfree( pFilpData ); + return 0; +} + +/*=========================================================================== +METHOD: + UserspaceRead (Public Method) + +DESCRIPTION: + Userspace read (synchronous) + +PARAMETERS + pFilp [ I ] - userspace file descriptor + pBuf [ I ] - read buffer + size [ I ] - size of read buffer + pUnusedFpos [ I ] - (unused) file position + +RETURN VALUE: + ssize_t - Number of bytes read for success + Negative errno for failure +===========================================================================*/ +ssize_t UserspaceRead( + struct file * pFilp, + char __user * pBuf, + size_t size, + loff_t * pUnusedFpos ) +{ + int result; + void * pReadData = NULL; + void * pSmallReadData; + sQMIFilpStorage * pFilpData = (sQMIFilpStorage *)pFilp->private_data; + + if (pFilpData == NULL) + { + DBG( "Bad file data\n" ); + return -EBADF; + } + + if (IsDeviceValid( pFilpData->mpDev ) == false) + { + DBG( "Invalid device! Updating f_ops\n" ); + pFilp->f_op = pFilp->f_dentry->d_inode->i_fop; + return -ENXIO; + } + + if (pFilpData->mClientID == (u16)-1) + { + DBG( "Client ID must be set before reading 0x%04X\n", + pFilpData->mClientID ); + return -EBADR; + } + + // Perform synchronous read + result = ReadSync( pFilpData->mpDev, + &pReadData, + pFilpData->mClientID, + 0 ); + if (result <= 0) + { + return result; + } + + // Discard QMUX header + result -= QMUXHeaderSize(); + pSmallReadData = pReadData + QMUXHeaderSize(); + + if (result > size) + { + DBG( "Read data is too large for amount user has requested\n" ); + kfree( pReadData ); + return -EOVERFLOW; + } + + DBG( "pBuf = 0x%p pSmallReadData = 0x%p, result = %d", + pBuf, pSmallReadData, result ); + + if (copy_to_user( pBuf, pSmallReadData, result ) != 0) + { + DBG( "Error copying read data to user\n" ); + result = -EFAULT; + } + + // Reader is responsible for freeing read buffer + kfree( pReadData ); + + return result; +} + +/*=========================================================================== +METHOD: + UserspaceWrite (Public Method) + +DESCRIPTION: + Userspace write (synchronous) + +PARAMETERS + pFilp [ I ] - userspace file descriptor + pBuf [ I ] - write buffer + size [ I ] - size of write buffer + pUnusedFpos [ I ] - (unused) file position + +RETURN VALUE: + ssize_t - Number of bytes read for success + Negative errno for failure +===========================================================================*/ +ssize_t UserspaceWrite( + struct file * pFilp, + const char __user * pBuf, + size_t size, + loff_t * pUnusedFpos ) +{ + int status; + void * pWriteBuffer; + sQMIFilpStorage * pFilpData = (sQMIFilpStorage *)pFilp->private_data; + + if (pFilpData == NULL) + { + DBG( "Bad file data\n" ); + return -EBADF; + } + + if (IsDeviceValid( pFilpData->mpDev ) == false) + { + DBG( "Invalid device! Updating f_ops\n" ); + pFilp->f_op = pFilp->f_dentry->d_inode->i_fop; + return -ENXIO; + } + + if (pFilpData->mClientID == (u16)-1) + { + DBG( "Client ID must be set before writing 0x%04X\n", + pFilpData->mClientID ); + return -EBADR; + } + + // Copy data from user to kernel space + pWriteBuffer = kmalloc( size + QMUXHeaderSize(), GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + status = copy_from_user( pWriteBuffer + QMUXHeaderSize(), pBuf, size ); + if (status != 0) + { + DBG( "Unable to copy data from userspace %d\n", status ); + kfree( pWriteBuffer ); + return status; + } + + status = WriteSync( pFilpData->mpDev, + pWriteBuffer, + size + QMUXHeaderSize(), + pFilpData->mClientID ); + + kfree( pWriteBuffer ); + + // On success, return requested size, not full QMI reqest size + if (status == size + QMUXHeaderSize()) + { + return size; + } + else + { + return status; + } +} + +/*=========================================================================== +METHOD: + UserspacePoll (Public Method) + +DESCRIPTION: + Used to determine if read/write operations are possible without blocking + +PARAMETERS + pFilp [ I ] - userspace file descriptor + pPollTable [I/O] - Wait object to notify the kernel when data + is ready + +RETURN VALUE: + unsigned int - bitmask of what operations can be done immediately +===========================================================================*/ +unsigned int UserspacePoll( + struct file * pFilp, + struct poll_table_struct * pPollTable ) +{ + sQMIFilpStorage * pFilpData = (sQMIFilpStorage *)pFilp->private_data; + sClientMemList * pClientMem; + unsigned long flags; + + // Always ready to write + unsigned long status = POLLOUT | POLLWRNORM; + + if (pFilpData == NULL) + { + DBG( "Bad file data\n" ); + return POLLERR; + } + + if (IsDeviceValid( pFilpData->mpDev ) == false) + { + DBG( "Invalid device! Updating f_ops\n" ); + pFilp->f_op = pFilp->f_dentry->d_inode->i_fop; + return POLLERR; + } + + if (pFilpData->mpDev->mbDeregisterQMIDevice) + { + DBG( "DeregisterQMIDevice ing\n" ); + return POLLHUP | POLLERR; + } + + if (pFilpData->mClientID == (u16)-1) + { + DBG( "Client ID must be set before polling 0x%04X\n", + pFilpData->mClientID ); + return POLLERR; + } + + // Critical section + spin_lock_irqsave( &pFilpData->mpDev->mQMIDev.mClientMemLock, flags ); + + // Get this client's memory location + pClientMem = FindClientMem( pFilpData->mpDev, + pFilpData->mClientID ); + if (pClientMem == NULL) + { + DBG( "Could not find this client's memory 0x%04X\n", + pFilpData->mClientID ); + + spin_unlock_irqrestore( &pFilpData->mpDev->mQMIDev.mClientMemLock, + flags ); + return POLLERR; + } + + poll_wait( pFilp, &pClientMem->mWaitQueue, pPollTable ); + + if (pClientMem->mpList != NULL) + { + status |= POLLIN | POLLRDNORM; + } + + // End critical section + spin_unlock_irqrestore( &pFilpData->mpDev->mQMIDev.mClientMemLock, flags ); + + // Always ready to write + return (status | POLLOUT | POLLWRNORM); +} + +/*=========================================================================*/ +// Initializer and destructor +/*=========================================================================*/ +int QMICTLSyncProc(sGobiUSBNet *pDev) +{ + void *pWriteBuffer; + void *pReadBuffer; + int result; + u16 writeBufferSize; + u8 transactionID; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return -EFAULT; + } + + writeBufferSize= QMICTLSyncReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + + transactionID = QMIXactionIDGet(pDev); + + /* send a QMI_CTL_SYNC_REQ (0x0027) */ + result = QMICTLSyncReq( pWriteBuffer, + writeBufferSize, + transactionID ); + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + QMICTL ); + + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + + // QMI CTL Sync Response + result = ReadSync( pDev, + &pReadBuffer, + QMICTL, + transactionID ); + if (result < 0) + { + return result; + } + + result = QMICTLSyncResp( pReadBuffer, + (u16)result ); + + kfree( pReadBuffer ); + + if (result < 0) /* need to re-sync */ + { + DBG( "sync response error code %d\n", result ); + /* start timer and wait for the response */ + /* process response */ + return result; + } + + // Success + return 0; +} + +static int qmi_sync_thread(void *data) { + sGobiUSBNet * pDev = (sGobiUSBNet *)data; + int result = 0; + +#if 1 + // Device is not ready for QMI connections right away + // Wait up to 30 seconds before failing + if (QMIReady( pDev, 30000 ) == false) + { + DBG( "Device unresponsive to QMI\n" ); + goto __qmi_sync_finished; + } + + // Initiate QMI CTL Sync Procedure + DBG( "Sending QMI CTL Sync Request\n" ); + result = QMICTLSyncProc(pDev); + if (result != 0) + { + DBG( "QMI CTL Sync Procedure Error\n" ); + goto __qmi_sync_finished; + } + else + { + DBG( "QMI CTL Sync Procedure Successful\n" ); + } + + // Setup Data Format + result = QMIWDASetDataFormat (pDev); + if (result != 0) + { + goto __qmi_sync_finished; + } + + // Setup WDS callback + result = SetupQMIWDSCallback( pDev ); + if (result != 0) + { + goto __qmi_sync_finished; + } + + // Fill MEID for device + result = QMIDMSGetMEID( pDev ); + if (result != 0) + { + goto __qmi_sync_finished; + } +#endif + +__qmi_sync_finished: + pDev->mbQMIReady = true; + complete_all(&pDev->mQMIReadyCompletion); + if (atomic_dec_and_test(&pDev->refcount)) { + kfree( pDev ); + } + return result; +} + +/*=========================================================================== +METHOD: + RegisterQMIDevice (Public Method) + +DESCRIPTION: + QMI Device initialization function + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + int - 0 for success + Negative errno for failure +===========================================================================*/ +int RegisterQMIDevice( sGobiUSBNet * pDev ) +{ + int result; + int GobiQMIIndex = 0; + dev_t devno; + char * pDevName; + + if (pDev->mQMIDev.mbCdevIsInitialized == true) + { + // Should never happen, but always better to check + DBG( "device already exists\n" ); + return -EEXIST; + } + + pDev->mbQMIValid = true; + pDev->mbDeregisterQMIDevice = false; + + // Set up for QMICTL + // (does not send QMI message, just sets up memory) + result = GetClientID( pDev, QMICTL ); + if (result != 0) + { + pDev->mbQMIValid = false; + return result; + } + atomic_set( &pDev->mQMIDev.mQMICTLTransactionID, 1 ); + + // Start Async reading + result = StartRead( pDev ); + if (result != 0) + { + pDev->mbQMIValid = false; + return result; + } + + if (pDev->mpNetDev->udev->descriptor.idVendor == cpu_to_le16(0x2c7c)) + { + usb_control_msg( pDev->mpNetDev->udev, + usb_sndctrlpipe( pDev->mpNetDev->udev, 0 ), + SET_CONTROL_LINE_STATE_REQUEST, + SET_CONTROL_LINE_STATE_REQUEST_TYPE, + CONTROL_DTR, + /* USB interface number to receive control message */ + pDev->mpIntf->cur_altsetting->desc.bInterfaceNumber, + NULL, + 0, + 100 ); + } + + //for EC21&25, must wait about 15 seconds to wait QMI ready. it is too long for driver probe(will block other drivers probe). + if (pDev->mpNetDev->udev->descriptor.idVendor == cpu_to_le16(0x2c7c)) + { + struct task_struct *qmi_sync_task; + atomic_inc(&pDev->refcount); + init_completion(&pDev->mQMIReadyCompletion); + pDev->mbQMIReady = false; + qmi_sync_task = kthread_run(qmi_sync_thread, (void *)pDev, "qmi_sync/%d", pDev->mpNetDev->udev->devnum); + if (IS_ERR(qmi_sync_task)) { + atomic_dec(&pDev->refcount); + DBG( "Create qmi_sync_thread fail\n" ); + return PTR_ERR(qmi_sync_task); + } + } + else + { + // Device is not ready for QMI connections right away + // Wait up to 30 seconds before failing + if (QMIReady( pDev, 30000 ) == false) + { + DBG( "Device unresponsive to QMI\n" ); + return -ETIMEDOUT; + } + + // Initiate QMI CTL Sync Procedure + DBG( "Sending QMI CTL Sync Request\n" ); + result = QMICTLSyncProc(pDev); + if (result != 0) + { + DBG( "QMI CTL Sync Procedure Error\n" ); + return result; + } + else + { + DBG( "QMI CTL Sync Procedure Successful\n" ); + } + + // Setup Data Format + result = QMIWDASetDataFormat (pDev); + if (result != 0) + { + return result; + } + + // Setup WDS callback + result = SetupQMIWDSCallback( pDev ); + if (result != 0) + { + return result; + } + + // Fill MEID for device + result = QMIDMSGetMEID( pDev ); + if (result != 0) + { + return result; + } + } + + // allocate and fill devno with numbers + result = alloc_chrdev_region( &devno, 0, 1, "qcqmi" ); + if (result < 0) + { + return result; + } + + // Create cdev + cdev_init( &pDev->mQMIDev.mCdev, &UserspaceQMIFops ); + pDev->mQMIDev.mCdev.owner = THIS_MODULE; + pDev->mQMIDev.mCdev.ops = &UserspaceQMIFops; + pDev->mQMIDev.mbCdevIsInitialized = true; + + result = cdev_add( &pDev->mQMIDev.mCdev, devno, 1 ); + if (result != 0) + { + DBG( "error adding cdev\n" ); + return result; + } + + // Match interface number (usb# or eth#) + if (!!(pDevName = strstr( pDev->mpNetDev->net->name, "eth" ))) { + pDevName += strlen( "eth" ); + } else if (!!(pDevName = strstr( pDev->mpNetDev->net->name, "usb" ))) { + pDevName += strlen( "usb" ); + } else { + DBG( "Bad net name: %s\n", pDev->mpNetDev->net->name ); + return -ENXIO; + } + GobiQMIIndex = simple_strtoul( pDevName, NULL, 10 ); + if (GobiQMIIndex < 0) + { + DBG( "Bad minor number\n" ); + return -ENXIO; + } + + // Always print this output + printk( KERN_INFO "creating qcqmi%d\n", + GobiQMIIndex ); + +#if (LINUX_VERSION_CODE >= KERNEL_VERSION( 2,6,27 )) + // kernel 2.6.27 added a new fourth parameter to device_create + // void * drvdata : the data to be added to the device for callbacks + device_create( pDev->mQMIDev.mpDevClass, + &pDev->mpIntf->dev, + devno, + NULL, + "qcqmi%d", + GobiQMIIndex ); +#else + device_create( pDev->mQMIDev.mpDevClass, + &pDev->mpIntf->dev, + devno, + "qcqmi%d", + GobiQMIIndex ); +#endif + + pDev->mQMIDev.mDevNum = devno; + + // Success + return 0; +} + +/*=========================================================================== +METHOD: + DeregisterQMIDevice (Public Method) + +DESCRIPTION: + QMI Device cleanup function + + NOTE: When this function is run the device is no longer valid + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + None +===========================================================================*/ +void DeregisterQMIDevice( sGobiUSBNet * pDev ) +{ + struct inode * pOpenInode; + struct list_head * pInodeList; + struct task_struct * pEachTask; + struct fdtable * pFDT; + struct file * pFilp; + unsigned long flags; + int count = 0; + int tries; + int result; + + // Should never happen, but check anyway + if (IsDeviceValid( pDev ) == false) + { + DBG( "wrong device\n" ); + return; + } + + pDev->mbDeregisterQMIDevice = true; + + // Release all clients + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + while (pDev->mQMIDev.mpClientMemList != NULL) + { + u16 mClientID = pDev->mQMIDev.mpClientMemList->mClientID; + if (waitqueue_active(&pDev->mQMIDev.mpClientMemList->mWaitQueue)) { + DBG("WaitQueue 0x%04X\n", mClientID); + wake_up_interruptible_sync( &pDev->mQMIDev.mpClientMemList->mWaitQueue ); + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + msleep(10); + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + continue; + } + + DBG( "release 0x%04X\n", pDev->mQMIDev.mpClientMemList->mClientID ); + + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + ReleaseClientID( pDev, mClientID ); + // NOTE: pDev->mQMIDev.mpClientMemList will + // be updated in ReleaseClientID() + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + } + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // Stop all reads + KillRead( pDev ); + + pDev->mbQMIValid = false; + + if (pDev->mQMIDev.mbCdevIsInitialized == false) + { + return; + } + + // Find each open file handle, and manually close it + + // Generally there will only be only one inode, but more are possible + list_for_each( pInodeList, &pDev->mQMIDev.mCdev.list ) + { + // Get the inode + pOpenInode = container_of( pInodeList, struct inode, i_devices ); + if (pOpenInode != NULL && (IS_ERR( pOpenInode ) == false)) + { + // Look for this inode in each task + + rcu_read_lock(); + for_each_process( pEachTask ) + { + task_lock(pEachTask); + if (pEachTask == NULL || pEachTask->files == NULL) + { + // Some tasks may not have files (e.g. Xsession) + task_unlock(pEachTask); + continue; + } + // For each file this task has open, check if it's referencing + // our inode. + spin_lock_irqsave( &pEachTask->files->file_lock, flags ); + task_unlock(pEachTask); //kernel/exit.c:do_exit() -> fs/file.c:exit_files() + pFDT = files_fdtable( pEachTask->files ); + for (count = 0; count < pFDT->max_fds; count++) + { + pFilp = pFDT->fd[count]; + if (pFilp != NULL && pFilp->f_dentry != NULL) + { + if (pFilp->f_dentry->d_inode == pOpenInode) + { + // Close this file handle + rcu_assign_pointer( pFDT->fd[count], NULL ); + spin_unlock_irqrestore( &pEachTask->files->file_lock, flags ); + + DBG( "forcing close of open file handle\n" ); + filp_close( pFilp, pEachTask->files ); + + spin_lock_irqsave( &pEachTask->files->file_lock, flags ); + } + } + } + spin_unlock_irqrestore( &pEachTask->files->file_lock, flags ); + } + rcu_read_unlock(); + } + } + + // Send SetControlLineState request (USB_CDC) + result = usb_control_msg( pDev->mpNetDev->udev, + usb_sndctrlpipe( pDev->mpNetDev->udev, 0 ), + SET_CONTROL_LINE_STATE_REQUEST, + SET_CONTROL_LINE_STATE_REQUEST_TYPE, + 0, // DTR not present + /* USB interface number to receive control message */ + pDev->mpIntf->cur_altsetting->desc.bInterfaceNumber, + NULL, + 0, + 100 ); + if (result < 0) + { + DBG( "Bad SetControlLineState status %d\n", result ); + } + + // Remove device (so no more calls can be made by users) + if (IS_ERR( pDev->mQMIDev.mpDevClass ) == false) + { + device_destroy( pDev->mQMIDev.mpDevClass, + pDev->mQMIDev.mDevNum ); + } + + // Hold onto cdev memory location until everyone is through using it. + // Timeout after 30 seconds (10 ms interval). Timeout should never happen, + // but exists to prevent an infinate loop just in case. + for (tries = 0; tries < 30 * 100; tries++) + { + int ref = atomic_read( &pDev->mQMIDev.mCdev.kobj.kref.refcount ); + if (ref > 1) + { + DBG( "cdev in use by %d tasks\n", ref - 1 ); + msleep( 10 ); + } + else + { + break; + } + } + + cdev_del( &pDev->mQMIDev.mCdev ); + + unregister_chrdev_region( pDev->mQMIDev.mDevNum, 1 ); + + return; +} + +/*=========================================================================*/ +// Driver level client management +/*=========================================================================*/ + +/*=========================================================================== +METHOD: + QMIReady (Public Method) + +DESCRIPTION: + Send QMI CTL GET VERSION INFO REQ and SET DATA FORMAT REQ + Wait for response or timeout + +PARAMETERS: + pDev [ I ] - Device specific memory + timeout [ I ] - Milliseconds to wait for response + +RETURN VALUE: + bool +===========================================================================*/ +bool QMIReady( + sGobiUSBNet * pDev, + u16 timeout ) +{ + int result; + void * pWriteBuffer; + u16 writeBufferSize; + void * pReadBuffer; + u16 readBufferSize; + struct semaphore readSem; + u16 curTime; + unsigned long flags; + u8 transactionID; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return false; + } + + writeBufferSize = QMICTLReadyReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return false; + } + + // An implimentation of down_timeout has not been agreed on, + // so it's been added and removed from the kernel several times. + // We're just going to ignore it and poll the semaphore. + + // Send a write every 1000 ms and see if we get a response + for (curTime = 0; curTime < timeout; curTime += 1000) + { + // Start read + sema_init( &readSem, 0 ); + + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + if (transactionID == 0) + { + transactionID = atomic_add_return( 1, &pDev->mQMIDev.mQMICTLTransactionID ); + } + result = ReadAsync( pDev, QMICTL, transactionID, UpSem, &readSem ); + if (result != 0) + { + kfree( pWriteBuffer ); + return false; + } + + // Fill buffer + result = QMICTLReadyReq( pWriteBuffer, + writeBufferSize, + transactionID ); + if (result < 0) + { + kfree( pWriteBuffer ); + return false; + } + + // Disregard status. On errors, just try again + WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + QMICTL ); + + msleep( 1000 ); + if (down_trylock( &readSem ) == 0) + { + // Enter critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Pop the read data + if (PopFromReadMemList( pDev, + QMICTL, + transactionID, + &pReadBuffer, + &readBufferSize ) == true) + { + // Success + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + // We don't care about the result + kfree( pReadBuffer ); + + break; + } + else + { + // Read mismatch/failure, unlock and continue + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + } + } + else + { + // Enter critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + // Timeout, remove the async read + NotifyAndPopNotifyList( pDev, QMICTL, transactionID ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + } + } + + kfree( pWriteBuffer ); + + // Did we time out? + if (curTime >= timeout) + { + return false; + } + + DBG( "QMI Ready after %u milliseconds\n", curTime ); + + // Success + return true; +} + +/*=========================================================================== +METHOD: + QMIWDSCallback (Public Method) + +DESCRIPTION: + QMI WDS callback function + Update net stats or link state + +PARAMETERS: + pDev [ I ] - Device specific memory + clientID [ I ] - Client ID + pData [ I ] - Callback data (unused) + +RETURN VALUE: + None +===========================================================================*/ +void QMIWDSCallback( + sGobiUSBNet * pDev, + u16 clientID, + void * pData ) +{ + bool bRet; + int result; + void * pReadBuffer; + u16 readBufferSize; + +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,31 )) + struct net_device_stats * pStats = &(pDev->mpNetDev->stats); +#else + struct net_device_stats * pStats = &(pDev->mpNetDev->net->stats); +#endif + + u32 TXOk = (u32)-1; + u32 RXOk = (u32)-1; + u32 TXErr = (u32)-1; + u32 RXErr = (u32)-1; + u32 TXOfl = (u32)-1; + u32 RXOfl = (u32)-1; + u64 TXBytesOk = (u64)-1; + u64 RXBytesOk = (u64)-1; + bool bLinkState; + bool bReconfigure; + unsigned long flags; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return; + } + + // Critical section + spin_lock_irqsave( &pDev->mQMIDev.mClientMemLock, flags ); + + bRet = PopFromReadMemList( pDev, + clientID, + 0, + &pReadBuffer, + &readBufferSize ); + + // End critical section + spin_unlock_irqrestore( &pDev->mQMIDev.mClientMemLock, flags ); + + if (bRet == false) + { + DBG( "WDS callback failed to get data\n" ); + return; + } + + // Default values + bLinkState = ! GobiTestDownReason( pDev, NO_NDIS_CONNECTION ); + bReconfigure = false; + + result = QMIWDSEventResp( pReadBuffer, + readBufferSize, + &TXOk, + &RXOk, + &TXErr, + &RXErr, + &TXOfl, + &RXOfl, + &TXBytesOk, + &RXBytesOk, + &bLinkState, + &bReconfigure ); + if (result < 0) + { + DBG( "bad WDS packet\n" ); + } + else + { + + // Fill in new values, ignore max values + if (TXOfl != (u32)-1) + { + pStats->tx_fifo_errors = TXOfl; + } + + if (RXOfl != (u32)-1) + { + pStats->rx_fifo_errors = RXOfl; + } + + if (TXErr != (u32)-1) + { + pStats->tx_errors = TXErr; + } + + if (RXErr != (u32)-1) + { + pStats->rx_errors = RXErr; + } + + if (TXOk != (u32)-1) + { + pStats->tx_packets = TXOk + pStats->tx_errors; + } + + if (RXOk != (u32)-1) + { + pStats->rx_packets = RXOk + pStats->rx_errors; + } + + if (TXBytesOk != (u64)-1) + { + pStats->tx_bytes = TXBytesOk; + } + + if (RXBytesOk != (u64)-1) + { + pStats->rx_bytes = RXBytesOk; + } + + if (bReconfigure == true) + { + DBG( "Net device link reset\n" ); + GobiSetDownReason( pDev, NO_NDIS_CONNECTION ); + GobiClearDownReason( pDev, NO_NDIS_CONNECTION ); + } + else + { + if (bLinkState == true) + { + if (GobiTestDownReason( pDev, NO_NDIS_CONNECTION )) { + DBG( "Net device link is connected\n" ); + GobiClearDownReason( pDev, NO_NDIS_CONNECTION ); + } + } + else + { + if (!GobiTestDownReason( pDev, NO_NDIS_CONNECTION )) { + DBG( "Net device link is disconnected\n" ); + GobiSetDownReason( pDev, NO_NDIS_CONNECTION ); + } + } + } + } + + kfree( pReadBuffer ); + + // Setup next read + result = ReadAsync( pDev, + clientID, + 0, + QMIWDSCallback, + pData ); + if (result != 0) + { + DBG( "unable to setup next async read\n" ); + } + + return; +} + +/*=========================================================================== +METHOD: + SetupQMIWDSCallback (Public Method) + +DESCRIPTION: + Request client and fire off reqests and start async read for + QMI WDS callback + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + int - 0 for success + Negative errno for failure +===========================================================================*/ +int SetupQMIWDSCallback( sGobiUSBNet * pDev ) +{ + int result; + void * pWriteBuffer; + u16 writeBufferSize; + u16 WDSClientID; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return -EFAULT; + } + + result = GetClientID( pDev, QMIWDS ); + if (result < 0) + { + return result; + } + WDSClientID = result; + + // QMI WDS Set Event Report + writeBufferSize = QMIWDSSetEventReportReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + + result = QMIWDSSetEventReportReq( pWriteBuffer, + writeBufferSize, + 1 ); + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + WDSClientID ); + kfree( pWriteBuffer ); + + if (result < 0) + { + return result; + } + + // QMI WDS Get PKG SRVC Status + writeBufferSize = QMIWDSGetPKGSRVCStatusReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + + result = QMIWDSGetPKGSRVCStatusReq( pWriteBuffer, + writeBufferSize, + 2 ); + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + WDSClientID ); + kfree( pWriteBuffer ); + + if (result < 0) + { + return result; + } + + // Setup asnyc read callback + result = ReadAsync( pDev, + WDSClientID, + 0, + QMIWDSCallback, + NULL ); + if (result != 0) + { + DBG( "unable to setup async read\n" ); + return result; + } + + // Send SetControlLineState request (USB_CDC) + // Required for Autoconnect + result = usb_control_msg( pDev->mpNetDev->udev, + usb_sndctrlpipe( pDev->mpNetDev->udev, 0 ), + SET_CONTROL_LINE_STATE_REQUEST, + SET_CONTROL_LINE_STATE_REQUEST_TYPE, + CONTROL_DTR, + /* USB interface number to receive control message */ + pDev->mpIntf->cur_altsetting->desc.bInterfaceNumber, + NULL, + 0, + 100 ); + if (result < 0) + { + DBG( "Bad SetControlLineState status %d\n", result ); + return result; + } + + return 0; +} + +/*=========================================================================== +METHOD: + QMIDMSGetMEID (Public Method) + +DESCRIPTION: + Register DMS client + send MEID req and parse response + Release DMS client + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + None +===========================================================================*/ +int QMIDMSGetMEID( sGobiUSBNet * pDev ) +{ + int result; + void * pWriteBuffer; + u16 writeBufferSize; + void * pReadBuffer; + u16 readBufferSize; + u16 DMSClientID; + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return -EFAULT; + } + + result = GetClientID( pDev, QMIDMS ); + if (result < 0) + { + return result; + } + DMSClientID = result; + + // QMI DMS Get Serial numbers Req + writeBufferSize = QMIDMSGetMEIDReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + + result = QMIDMSGetMEIDReq( pWriteBuffer, + writeBufferSize, + 1 ); + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + DMSClientID ); + kfree( pWriteBuffer ); + + if (result < 0) + { + return result; + } + + // QMI DMS Get Serial numbers Resp + result = ReadSync( pDev, + &pReadBuffer, + DMSClientID, + 1 ); + if (result < 0) + { + return result; + } + readBufferSize = result; + + result = QMIDMSGetMEIDResp( pReadBuffer, + readBufferSize, + &pDev->mMEID[0], + 14 ); + kfree( pReadBuffer ); + + if (result < 0) + { + DBG( "bad get MEID resp\n" ); + + // Non fatal error, device did not return any MEID + // Fill with 0's + memset( &pDev->mMEID[0], '0', 14 ); + } + + ReleaseClientID( pDev, DMSClientID ); + + // Success + return 0; +} + +/*=========================================================================== +METHOD: + QMIWDASetDataFormat (Public Method) + +DESCRIPTION: + Register WDA client + send Data format request and parse response + Release WDA client + +PARAMETERS: + pDev [ I ] - Device specific memory + +RETURN VALUE: + None +===========================================================================*/ +int QMIWDASetDataFormat( sGobiUSBNet * pDev ) +{ + int result; + void * pWriteBuffer; + u16 writeBufferSize; + void * pReadBuffer; + u16 readBufferSize; + u16 WDAClientID; + + DBG("\n"); + + if (IsDeviceValid( pDev ) == false) + { + DBG( "Invalid device\n" ); + return -EFAULT; + } + + result = GetClientID( pDev, QMIWDA ); + if (result < 0) + { + return result; + } + WDAClientID = result; + + // QMI WDA Set Data Format Request + writeBufferSize = QMIWDASetDataFormatReqSize(); + pWriteBuffer = kmalloc( writeBufferSize, GFP_KERNEL ); + if (pWriteBuffer == NULL) + { + return -ENOMEM; + } + + result = QMIWDASetDataFormatReq( pWriteBuffer, + writeBufferSize, + 1 ); + if (result < 0) + { + kfree( pWriteBuffer ); + return result; + } + + result = WriteSync( pDev, + pWriteBuffer, + writeBufferSize, + WDAClientID ); + kfree( pWriteBuffer ); + + if (result < 0) + { + return result; + } + + // QMI DMS Get Serial numbers Resp + result = ReadSync( pDev, + &pReadBuffer, + WDAClientID, + 1 ); + if (result < 0) + { + return result; + } + readBufferSize = result; + + result = QMIWDASetDataFormatResp( pReadBuffer, + readBufferSize ); + + kfree( pReadBuffer ); + +#if 1 //def DATA_MODE_RP + pDev->mbRawIPMode = (result == 2); + if (pDev->mbRawIPMode) { + pDev->mpNetDev->net->flags |= IFF_NOARP; + } +#endif + + if (result < 0) + { + DBG( "Data Format Cannot be set\n" ); + } + + ReleaseClientID( pDev, WDAClientID ); + + // Success + return 0; +} diff --git a/drivers/net/usb/QMIDevice.h b/drivers/net/usb/QMIDevice.h new file mode 100644 index 0000000..f28fe30 --- /dev/null +++ b/drivers/net/usb/QMIDevice.h @@ -0,0 +1,345 @@ +/*=========================================================================== +FILE: + QMIDevice.h + +DESCRIPTION: + Functions related to the QMI interface device + +FUNCTIONS: + Generic functions + IsDeviceValid + PrintHex + GobiSetDownReason + GobiClearDownReason + GobiTestDownReason + + Driver level asynchronous read functions + ResubmitIntURB + ReadCallback + IntCallback + StartRead + KillRead + + Internal read/write functions + ReadAsync + UpSem + ReadSync + WriteSyncCallback + WriteSync + + Internal memory management functions + GetClientID + ReleaseClientID + FindClientMem + AddToReadMemList + PopFromReadMemList + AddToNotifyList + NotifyAndPopNotifyList + AddToURBList + PopFromURBList + + Internal userspace wrapper functions + UserspaceunlockedIOCTL + + Userspace wrappers + UserspaceOpen + UserspaceIOCTL + UserspaceClose + UserspaceRead + UserspaceWrite + UserspacePoll + + Initializer and destructor + RegisterQMIDevice + DeregisterQMIDevice + + Driver level client management + QMIReady + QMIWDSCallback + SetupQMIWDSCallback + QMIDMSGetMEID + +Copyright (c) 2011, Code Aurora Forum. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Code Aurora Forum nor + the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written + permission. + + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +===========================================================================*/ + +//--------------------------------------------------------------------------- +// Pragmas +//--------------------------------------------------------------------------- +#pragma once + +//--------------------------------------------------------------------------- +// Include Files +//--------------------------------------------------------------------------- +#include "Structs.h" +#include "QMI.h" + +/*=========================================================================*/ +// Generic functions +/*=========================================================================*/ + +// Basic test to see if device memory is valid +bool IsDeviceValid( sGobiUSBNet * pDev ); + +// Print Hex data, for debug purposes +void PrintHex( + void * pBuffer, + u16 bufSize ); + +// Sets mDownReason and turns carrier off +void GobiSetDownReason( + sGobiUSBNet * pDev, + u8 reason ); + +// Clear mDownReason and may turn carrier on +void GobiClearDownReason( + sGobiUSBNet * pDev, + u8 reason ); + +// Tests mDownReason and returns whether reason is set +bool GobiTestDownReason( + sGobiUSBNet * pDev, + u8 reason ); + +/*=========================================================================*/ +// Driver level asynchronous read functions +/*=========================================================================*/ + +// Resubmit interrupt URB, re-using same values +int ResubmitIntURB( struct urb * pIntURB ); + +// Read callback +// Put the data in storage and notify anyone waiting for data +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void ReadCallback( struct urb * pReadURB ); +#else +void ReadCallback(struct urb *pReadURB, struct pt_regs *regs); +#endif + +// Inturrupt callback +// Data is available, start a read URB +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void IntCallback( struct urb * pIntURB ); +#else +void IntCallback(struct urb *pIntURB, struct pt_regs *regs); +#endif + +// Start continuous read "thread" +int StartRead( sGobiUSBNet * pDev ); + +// Kill continuous read "thread" +void KillRead( sGobiUSBNet * pDev ); + +/*=========================================================================*/ +// Internal read/write functions +/*=========================================================================*/ + +// Start asynchronous read +// Reading client's data store, not device +int ReadAsync( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void (*pCallback)(sGobiUSBNet *, u16, void *), + void * pData ); + +// Notification function for synchronous read +void UpSem( + sGobiUSBNet * pDev, + u16 clientID, + void * pData ); + +// Start synchronous read +// Reading client's data store, not device +int ReadSync( + sGobiUSBNet * pDev, + void ** ppOutBuffer, + u16 clientID, + u16 transactionID ); + +// Write callback +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +void WriteSyncCallback( struct urb * pWriteURB ); +#else +void WriteSyncCallback(struct urb *pWriteURB, struct pt_regs *regs); +#endif + +// Start synchronous write +int WriteSync( + sGobiUSBNet * pDev, + char * pInWriteBuffer, + int size, + u16 clientID ); + +/*=========================================================================*/ +// Internal memory management functions +/*=========================================================================*/ + +// Create client and allocate memory +int GetClientID( + sGobiUSBNet * pDev, + u8 serviceType ); + +// Release client and free memory +void ReleaseClientID( + sGobiUSBNet * pDev, + u16 clientID ); + +// Find this client's memory +sClientMemList * FindClientMem( + sGobiUSBNet * pDev, + u16 clientID ); + +// Add Data to this client's ReadMem list +bool AddToReadMemList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void * pData, + u16 dataSize ); + +// Remove data from this client's ReadMem list if it matches +// the specified transaction ID. +bool PopFromReadMemList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void ** ppData, + u16 * pDataSize ); + +// Add Notify entry to this client's notify List +bool AddToNotifyList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID, + void (* pNotifyFunct)(sGobiUSBNet *, u16, void *), + void * pData ); + +// Remove first Notify entry from this client's notify list +// and Run function +bool NotifyAndPopNotifyList( + sGobiUSBNet * pDev, + u16 clientID, + u16 transactionID ); + +// Add URB to this client's URB list +bool AddToURBList( + sGobiUSBNet * pDev, + u16 clientID, + struct urb * pURB ); + +// Remove URB from this client's URB list +struct urb * PopFromURBList( + sGobiUSBNet * pDev, + u16 clientID ); + +/*=========================================================================*/ +// Internal userspace wrappers +/*=========================================================================*/ + +// Userspace unlocked ioctl +long UserspaceunlockedIOCTL( + struct file * pFilp, + unsigned int cmd, + unsigned long arg ); + +/*=========================================================================*/ +// Userspace wrappers +/*=========================================================================*/ + +// Userspace open +int UserspaceOpen( + struct inode * pInode, + struct file * pFilp ); + +// Userspace ioctl +int UserspaceIOCTL( + struct inode * pUnusedInode, + struct file * pFilp, + unsigned int cmd, + unsigned long arg ); + +// Userspace close +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,14 )) +int UserspaceClose( + struct file * pFilp, + fl_owner_t unusedFileTable ); +#else +int UserspaceClose( struct file * pFilp ); +#endif + +// Userspace read (synchronous) +ssize_t UserspaceRead( + struct file * pFilp, + char __user * pBuf, + size_t size, + loff_t * pUnusedFpos ); + +// Userspace write (synchronous) +ssize_t UserspaceWrite( + struct file * pFilp, + const char __user * pBuf, + size_t size, + loff_t * pUnusedFpos ); + +unsigned int UserspacePoll( + struct file * pFilp, + struct poll_table_struct * pPollTable ); + +/*=========================================================================*/ +// Initializer and destructor +/*=========================================================================*/ + +// QMI Device initialization function +int RegisterQMIDevice( sGobiUSBNet * pDev ); + +// QMI Device cleanup function +void DeregisterQMIDevice( sGobiUSBNet * pDev ); + +/*=========================================================================*/ +// Driver level client management +/*=========================================================================*/ + +// Check if QMI is ready for use +bool QMIReady( + sGobiUSBNet * pDev, + u16 timeout ); + +// QMI WDS callback function +void QMIWDSCallback( + sGobiUSBNet * pDev, + u16 clientID, + void * pData ); + +// Fire off reqests and start async read for QMI WDS callback +int SetupQMIWDSCallback( sGobiUSBNet * pDev ); + +// Register client, send req and parse MEID response, release client +int QMIDMSGetMEID( sGobiUSBNet * pDev ); + +// Register client, send req and parse Data format response, release client +int QMIWDASetDataFormat( sGobiUSBNet * pDev ); diff --git a/drivers/net/usb/Structs.h b/drivers/net/usb/Structs.h new file mode 100644 index 0000000..8572401 --- /dev/null +++ b/drivers/net/usb/Structs.h @@ -0,0 +1,423 @@ +/*=========================================================================== +FILE: + Structs.h + +DESCRIPTION: + Declaration of structures used by the Qualcomm Linux USB Network driver + +FUNCTIONS: + none + +Copyright (c) 2011, Code Aurora Forum. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of Code Aurora Forum nor + the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written + permission. + + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. +===========================================================================*/ + +//--------------------------------------------------------------------------- +// Pragmas +//--------------------------------------------------------------------------- +#pragma once + +//--------------------------------------------------------------------------- +// Include Files +//--------------------------------------------------------------------------- +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,22 )) +#define bool u8 +#define URB_FREE_BUFFER 0x0100 /* Free transfer buffer with the URB */ + +/** + * usb_endpoint_type - get the endpoint's transfer type + * @epd: endpoint to be checked + * + * Returns one of USB_ENDPOINT_XFER_{CONTROL, ISOC, BULK, INT} according + * to @epd's transfer type. + */ +static inline int usb_endpoint_type(const struct usb_endpoint_descriptor *epd) +{ + return epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK; +} +#endif + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,14 )) +/** + * usb_endpoint_dir_in - check if the endpoint has IN direction + * @epd: endpoint to be checked + * + * Returns true if the endpoint is of type IN, otherwise it returns false. + */ +static inline int usb_endpoint_dir_in(const struct usb_endpoint_descriptor *epd) +{ + return ((epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN); +} + +/** + * usb_endpoint_dir_out - check if the endpoint has OUT direction + * @epd: endpoint to be checked + * + * Returns true if the endpoint is of type OUT, otherwise it returns false. + */ +static inline int usb_endpoint_dir_out( + const struct usb_endpoint_descriptor *epd) +{ + return ((epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT); +} + +/** + * usb_endpoint_xfer_int - check if the endpoint has interrupt transfer type + * @epd: endpoint to be checked + * + * Returns true if the endpoint is of type interrupt, otherwise it returns + * false. + */ +static inline int usb_endpoint_xfer_int( + const struct usb_endpoint_descriptor *epd) +{ + return ((epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == + USB_ENDPOINT_XFER_INT); +} + +static inline int usb_autopm_set_interface(struct usb_interface *intf) +{ return 0; } + +static inline int usb_autopm_get_interface(struct usb_interface *intf) +{ return 0; } + +static inline int usb_autopm_get_interface_async(struct usb_interface *intf) +{ return 0; } + +static inline void usb_autopm_put_interface(struct usb_interface *intf) +{ } +static inline void usb_autopm_put_interface_async(struct usb_interface *intf) +{ } +static inline void usb_autopm_enable(struct usb_interface *intf) +{ } +static inline void usb_autopm_disable(struct usb_interface *intf) +{ } +static inline void usb_mark_last_busy(struct usb_device *udev) +{ } +#endif + +#if (LINUX_VERSION_CODE <= KERNEL_VERSION( 2,6,24 )) + #include "usbnet.h" +#else + #include +#endif + +#if (LINUX_VERSION_CODE > KERNEL_VERSION( 2,6,25 )) + #include +#else + #include +#endif + +// Used in recursion, defined later below +struct sGobiUSBNet; + +/*=========================================================================*/ +// Struct sReadMemList +// +// Structure that defines an entry in a Read Memory linked list +/*=========================================================================*/ +typedef struct sReadMemList +{ + /* Data buffer */ + void * mpData; + + /* Transaction ID */ + u16 mTransactionID; + + /* Size of data buffer */ + u16 mDataSize; + + /* Next entry in linked list */ + struct sReadMemList * mpNext; + +} sReadMemList; + +/*=========================================================================*/ +// Struct sNotifyList +// +// Structure that defines an entry in a Notification linked list +/*=========================================================================*/ +typedef struct sNotifyList +{ + /* Function to be run when data becomes available */ + void (* mpNotifyFunct)(struct sGobiUSBNet *, u16, void *); + + /* Transaction ID */ + u16 mTransactionID; + + /* Data to provide as parameter to mpNotifyFunct */ + void * mpData; + + /* Next entry in linked list */ + struct sNotifyList * mpNext; + +} sNotifyList; + +/*=========================================================================*/ +// Struct sURBList +// +// Structure that defines an entry in a URB linked list +/*=========================================================================*/ +typedef struct sURBList +{ + /* The current URB */ + struct urb * mpURB; + + /* Next entry in linked list */ + struct sURBList * mpNext; + +} sURBList; + +/*=========================================================================*/ +// Struct sClientMemList +// +// Structure that defines an entry in a Client Memory linked list +// Stores data specific to a Service Type and Client ID +/*=========================================================================*/ +typedef struct sClientMemList +{ + /* Client ID for this Client */ + u16 mClientID; + + /* Linked list of Read entries */ + /* Stores data read from device before sending to client */ + sReadMemList * mpList; + + /* Linked list of Notification entries */ + /* Stores notification functions to be run as data becomes + available or the device is removed */ + sNotifyList * mpReadNotifyList; + + /* Linked list of URB entries */ + /* Stores pointers to outstanding URBs which need canceled + when the client is deregistered or the device is removed */ + sURBList * mpURBList; + + /* Next entry in linked list */ + struct sClientMemList * mpNext; + + /* Wait queue object for poll() */ + wait_queue_head_t mWaitQueue; + +} sClientMemList; + +/*=========================================================================*/ +// Struct sURBSetupPacket +// +// Structure that defines a USB Setup packet for Control URBs +// Taken from USB CDC specifications +/*=========================================================================*/ +typedef struct sURBSetupPacket +{ + /* Request type */ + u8 mRequestType; + + /* Request code */ + u8 mRequestCode; + + /* Value */ + u16 mValue; + + /* Index */ + u16 mIndex; + + /* Length of Control URB */ + u16 mLength; + +} sURBSetupPacket; + +// Common value for sURBSetupPacket.mLength +#define DEFAULT_READ_URB_LENGTH 0x1000 + +#ifdef CONFIG_PM +#if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) +/*=========================================================================*/ +// Struct sAutoPM +// +// Structure used to manage AutoPM thread which determines whether the +// device is in use or may enter autosuspend. Also submits net +// transmissions asynchronously. +/*=========================================================================*/ +typedef struct sAutoPM +{ + /* Thread for atomic autopm function */ + struct task_struct * mpThread; + + /* Signal for completion when it's time for the thread to work */ + struct completion mThreadDoWork; + + /* Time to exit? */ + bool mbExit; + + /* List of URB's queued to be sent to the device */ + sURBList * mpURBList; + + /* URB list lock (for adding and removing elements) */ + spinlock_t mURBListLock; + + /* Length of the URB list */ + atomic_t mURBListLen; + + /* Active URB */ + struct urb * mpActiveURB; + + /* Active URB lock (for adding and removing elements) */ + spinlock_t mActiveURBLock; + + /* Duplicate pointer to USB device interface */ + struct usb_interface * mpIntf; + +} sAutoPM; +#endif +#endif /* CONFIG_PM */ + +/*=========================================================================*/ +// Struct sQMIDev +// +// Structure that defines the data for the QMI device +/*=========================================================================*/ +typedef struct sQMIDev +{ + /* Device number */ + dev_t mDevNum; + + /* Device class */ + struct class * mpDevClass; + + /* cdev struct */ + struct cdev mCdev; + + /* is mCdev initialized? */ + bool mbCdevIsInitialized; + + /* Pointer to read URB */ + struct urb * mpReadURB; + + /* Read setup packet */ + sURBSetupPacket * mpReadSetupPacket; + + /* Read buffer attached to current read URB */ + void * mpReadBuffer; + + /* Inturrupt URB */ + /* Used to asynchronously notify when read data is available */ + struct urb * mpIntURB; + + /* Buffer used by Inturrupt URB */ + void * mpIntBuffer; + + /* Pointer to memory linked list for all clients */ + sClientMemList * mpClientMemList; + + /* Spinlock for client Memory entries */ + spinlock_t mClientMemLock; + + /* Transaction ID associated with QMICTL "client" */ + atomic_t mQMICTLTransactionID; + +} sQMIDev; + +/*=========================================================================*/ +// Struct sGobiUSBNet +// +// Structure that defines the data associated with the Qualcomm USB device +/*=========================================================================*/ +typedef struct sGobiUSBNet +{ + atomic_t refcount; + + /* Net device structure */ + struct usbnet * mpNetDev; + +#if 1 //def DATA_MODE_RP + /* QMI "device" work in IP Mode or ETH Mode */ + bool mbRawIPMode; +#endif + + struct completion mQMIReadyCompletion; + bool mbQMIReady; + + /* Usb device interface */ + struct usb_interface * mpIntf; + + /* Pointers to usbnet_open and usbnet_stop functions */ + int (* mpUSBNetOpen)(struct net_device *); + int (* mpUSBNetStop)(struct net_device *); + + /* Reason(s) why interface is down */ + /* Used by Gobi*DownReason */ + unsigned long mDownReason; +#define NO_NDIS_CONNECTION 0 +#define CDC_CONNECTION_SPEED 1 +#define DRIVER_SUSPENDED 2 +#define NET_IFACE_STOPPED 3 + + /* QMI "device" status */ + bool mbQMIValid; + + bool mbDeregisterQMIDevice; + + /* QMI "device" memory */ + sQMIDev mQMIDev; + + /* Device MEID */ + char mMEID[14]; + +#ifdef CONFIG_PM + #if (LINUX_VERSION_CODE < KERNEL_VERSION( 2,6,29 )) + /* AutoPM thread */ + sAutoPM mAutoPM; +#endif +#endif /* CONFIG_PM */ +} sGobiUSBNet; + +/*=========================================================================*/ +// Struct sQMIFilpStorage +// +// Structure that defines the storage each file handle contains +// Relates the file handle to a client +/*=========================================================================*/ +typedef struct sQMIFilpStorage +{ + /* Client ID */ + u16 mClientID; + + /* Device pointer */ + sGobiUSBNet * mpDev; + +} sQMIFilpStorage; + diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c new file mode 100644 index 0000000..235d676 --- /dev/null +++ b/drivers/net/usb/qmi_wwan.c @@ -0,0 +1,1142 @@ +/* + * Copyright (c) 2012 Bjørn Mork + * + * The probing code is heavily inspired by cdc_ether, which is: + * Copyright (C) 2003-2005 by David Brownell + * Copyright (C) 2006 by Ole Andre Vadla Ravnas (ActiveSync) + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * version 2 as published by the Free Software Foundation. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* This driver supports wwan (3G/LTE/?) devices using a vendor + * specific management protocol called Qualcomm MSM Interface (QMI) - + * in addition to the more common AT commands over serial interface + * management + * + * QMI is wrapped in CDC, using CDC encapsulated commands on the + * control ("master") interface of a two-interface CDC Union + * resembling standard CDC ECM. The devices do not use the control + * interface for any other CDC messages. Most likely because the + * management protocol is used in place of the standard CDC + * notifications NOTIFY_NETWORK_CONNECTION and NOTIFY_SPEED_CHANGE + * + * Alternatively, control and data functions can be combined in a + * single USB interface. + * + * Handling a protocol like QMI is out of the scope for any driver. + * It is exported as a character device using the cdc-wdm driver as + * a subdriver, enabling userspace applications ("modem managers") to + * handle it. + * + * These devices may alternatively/additionally be configured using AT + * commands on a serial interface + */ + +/* driver specific data */ +struct qmi_wwan_state { + struct usb_driver *subdriver; + atomic_t pmcount; + unsigned long flags; + struct usb_interface *control; + struct usb_interface *data; +}; + +enum qmi_wwan_flags { + QMI_WWAN_FLAG_RAWIP = 1 << 0, +}; + +enum qmi_wwan_quirks { + QMI_WWAN_QUIRK_DTR = 1 << 0, /* needs "set DTR" request */ +}; + +static void qmi_wwan_netdev_setup(struct net_device *net) +{ + struct usbnet *dev = netdev_priv(net); + struct qmi_wwan_state *info = (void *)&dev->data; + + if (info->flags & QMI_WWAN_FLAG_RAWIP) { + net->header_ops = NULL; /* No header */ + net->type = ARPHRD_NONE; + net->hard_header_len = 0; + net->addr_len = 0; + net->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST; + set_bit(EVENT_NO_IP_ALIGN, &dev->flags); + netdev_dbg(net, "mode: raw IP\n"); + } else if (!net->header_ops) { /* don't bother if already set */ + ether_setup(net); + clear_bit(EVENT_NO_IP_ALIGN, &dev->flags); + netdev_dbg(net, "mode: Ethernet\n"); + } + + /* recalculate buffers after changing hard_header_len */ + usbnet_change_mtu(net, net->mtu); +} + +static ssize_t raw_ip_show(struct device *d, struct device_attribute *attr, char *buf) +{ + struct usbnet *dev = netdev_priv(to_net_dev(d)); + struct qmi_wwan_state *info = (void *)&dev->data; + + return sprintf(buf, "%c\n", info->flags & QMI_WWAN_FLAG_RAWIP ? 'Y' : 'N'); +} + +static ssize_t raw_ip_store(struct device *d, struct device_attribute *attr, const char *buf, size_t len) +{ + struct usbnet *dev = netdev_priv(to_net_dev(d)); + struct qmi_wwan_state *info = (void *)&dev->data; + bool enable; + int ret; + + if (strtobool(buf, &enable)) + return -EINVAL; + + /* no change? */ + if (enable == (info->flags & QMI_WWAN_FLAG_RAWIP)) + return len; + + if (!rtnl_trylock()) + return restart_syscall(); + + /* we don't want to modify a running netdev */ + if (netif_running(dev->net)) { + netdev_err(dev->net, "Cannot change a running device\n"); + ret = -EBUSY; + goto err; + } + + /* let other drivers deny the change */ + ret = call_netdevice_notifiers(NETDEV_PRE_TYPE_CHANGE, dev->net); + ret = notifier_to_errno(ret); + if (ret) { + netdev_err(dev->net, "Type change was refused\n"); + goto err; + } + + if (enable) + info->flags |= QMI_WWAN_FLAG_RAWIP; + else + info->flags &= ~QMI_WWAN_FLAG_RAWIP; + qmi_wwan_netdev_setup(dev->net); + call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev->net); + ret = len; +err: + rtnl_unlock(); + return ret; +} + +static DEVICE_ATTR_RW(raw_ip); + +static struct attribute *qmi_wwan_sysfs_attrs[] = { + &dev_attr_raw_ip.attr, + NULL, +}; + +static struct attribute_group qmi_wwan_sysfs_attr_group = { + .name = "qmi", + .attrs = qmi_wwan_sysfs_attrs, +}; + +/* default ethernet address used by the modem */ +static const u8 default_modem_addr[ETH_ALEN] = {0x02, 0x50, 0xf3}; + +static const u8 buggy_fw_addr[ETH_ALEN] = {0x00, 0xa0, 0xc6, 0x00, 0x00, 0x00}; + +/* Make up an ethernet header if the packet doesn't have one. + * + * A firmware bug common among several devices cause them to send raw + * IP packets under some circumstances. There is no way for the + * driver/host to know when this will happen. And even when the bug + * hits, some packets will still arrive with an intact header. + * + * The supported devices are only capably of sending IPv4, IPv6 and + * ARP packets on a point-to-point link. Any packet with an ethernet + * header will have either our address or a broadcast/multicast + * address as destination. ARP packets will always have a header. + * + * This means that this function will reliably add the appropriate + * header iff necessary, provided our hardware address does not start + * with 4 or 6. + * + * Another common firmware bug results in all packets being addressed + * to 00:a0:c6:00:00:00 despite the host address being different. + * This function will also fixup such packets. + */ + +#if 1 /* Added by Quectel */ +#include +struct sk_buff *qmi_wwan_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags) +{ + if (dev->udev->descriptor.idVendor != cpu_to_le16(0x2C7C)) + return skb; + + /* Skip Ethernet header from message */ + if (skb_pull(skb, ETH_HLEN)) { + return skb; + } else { + dev_err(&dev->intf->dev, "Packet Dropped "); + } + + /* Filter the packet out, release it */ + dev_kfree_skb_any(skb); + return NULL; +} +#endif + +static int qmi_wwan_rx_fixup(struct usbnet *dev, struct sk_buff *skb) +{ + struct qmi_wwan_state *info = (void *)&dev->data; + bool rawip = info->flags & QMI_WWAN_FLAG_RAWIP; + __be16 proto; + + /* This check is no longer done by usbnet */ + if (skb->len < dev->net->hard_header_len) + return 0; + + switch (skb->data[0] & 0xf0) { + case 0x40: + proto = htons(ETH_P_IP); + break; + case 0x60: + proto = htons(ETH_P_IPV6); + break; + case 0x00: + if (rawip) + return 0; + if (is_multicast_ether_addr(skb->data)) + return 1; + /* possibly bogus destination - rewrite just in case */ + skb_reset_mac_header(skb); + goto fix_dest; + default: + if (rawip) + return 0; + /* pass along other packets without modifications */ + return 1; + } + if (rawip) { + skb_reset_mac_header(skb); + skb->dev = dev->net; /* normally set by eth_type_trans */ + skb->protocol = proto; + return 1; + } + + if (skb_headroom(skb) < ETH_HLEN) + return 0; + skb_push(skb, ETH_HLEN); + skb_reset_mac_header(skb); + eth_hdr(skb)->h_proto = proto; + eth_zero_addr(eth_hdr(skb)->h_source); +fix_dest: + memcpy(eth_hdr(skb)->h_dest, dev->net->dev_addr, ETH_ALEN); + return 1; +} + +/* very simplistic detection of IPv4 or IPv6 headers */ +static bool possibly_iphdr(const char *data) +{ + return (data[0] & 0xd0) == 0x40; +} + +/* disallow addresses which may be confused with IP headers */ +static int qmi_wwan_mac_addr(struct net_device *dev, void *p) +{ + int ret; + struct sockaddr *addr = p; + + ret = eth_prepare_mac_addr_change(dev, p); + if (ret < 0) + return ret; + if (possibly_iphdr(addr->sa_data)) + return -EADDRNOTAVAIL; + eth_commit_mac_addr_change(dev, p); + return 0; +} + +static const struct net_device_ops qmi_wwan_netdev_ops = { + .ndo_open = usbnet_open, + .ndo_stop = usbnet_stop, + .ndo_start_xmit = usbnet_start_xmit, + .ndo_tx_timeout = usbnet_tx_timeout, + .ndo_change_mtu = usbnet_change_mtu, + .ndo_set_mac_address = qmi_wwan_mac_addr, + .ndo_validate_addr = eth_validate_addr, +}; + +/* using a counter to merge subdriver requests with our own into a + * combined state + */ +static int qmi_wwan_manage_power(struct usbnet *dev, int on) +{ + struct qmi_wwan_state *info = (void *)&dev->data; + int rv; + + dev_dbg(&dev->intf->dev, "%s() pmcount=%d, on=%d\n", __func__, + atomic_read(&info->pmcount), on); + + if ((on && atomic_add_return(1, &info->pmcount) == 1) || + (!on && atomic_dec_and_test(&info->pmcount))) { + /* need autopm_get/put here to ensure the usbcore sees + * the new value + */ + rv = usb_autopm_get_interface(dev->intf); + dev->intf->needs_remote_wakeup = on; + if (!rv) + usb_autopm_put_interface(dev->intf); + } + return 0; +} + +static int qmi_wwan_cdc_wdm_manage_power(struct usb_interface *intf, int on) +{ + struct usbnet *dev = usb_get_intfdata(intf); + + /* can be called while disconnecting */ + if (!dev) + return 0; + return qmi_wwan_manage_power(dev, on); +} + +/* collect all three endpoints and register subdriver */ +static int qmi_wwan_register_subdriver(struct usbnet *dev) +{ + int rv; + struct usb_driver *subdriver = NULL; + struct qmi_wwan_state *info = (void *)&dev->data; + + /* collect bulk endpoints */ + rv = usbnet_get_endpoints(dev, info->data); + if (rv < 0) + goto err; + + /* update status endpoint if separate control interface */ + if (info->control != info->data) + dev->status = &info->control->cur_altsetting->endpoint[0]; + + /* require interrupt endpoint for subdriver */ + if (!dev->status) { + rv = -EINVAL; + goto err; + } + + /* for subdriver power management */ + atomic_set(&info->pmcount, 0); + + /* register subdriver */ + subdriver = usb_cdc_wdm_register(info->control, &dev->status->desc, + 4096, &qmi_wwan_cdc_wdm_manage_power); + if (IS_ERR(subdriver)) { + dev_err(&info->control->dev, "subdriver registration failed\n"); + rv = PTR_ERR(subdriver); + goto err; + } + + /* prevent usbnet from using status endpoint */ + dev->status = NULL; + + /* save subdriver struct for suspend/resume wrappers */ + info->subdriver = subdriver; + +err: + return rv; +} + +/* Send CDC SetControlLineState request, setting or clearing the DTR. + * "Required for Autoconnect and 9x30 to wake up" according to the + * GobiNet driver. The requirement has been verified on an MDM9230 + * based Sierra Wireless MC7455 + */ +static int qmi_wwan_change_dtr(struct usbnet *dev, bool on) +{ + u8 intf = dev->intf->cur_altsetting->desc.bInterfaceNumber; + + return usbnet_write_cmd(dev, USB_CDC_REQ_SET_CONTROL_LINE_STATE, + USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE, + on ? 0x01 : 0x00, intf, NULL, 0); +} + +static int qmi_wwan_bind(struct usbnet *dev, struct usb_interface *intf) +{ + int status = -1; + u8 *buf = intf->cur_altsetting->extra; + int len = intf->cur_altsetting->extralen; + struct usb_interface_descriptor *desc = &intf->cur_altsetting->desc; + struct usb_cdc_union_desc *cdc_union; + struct usb_cdc_ether_desc *cdc_ether; + struct usb_driver *driver = driver_of(intf); + struct qmi_wwan_state *info = (void *)&dev->data; + struct usb_cdc_parsed_header hdr; + + BUILD_BUG_ON((sizeof(((struct usbnet *)0)->data) < + sizeof(struct qmi_wwan_state))); + + /* set up initial state */ + info->control = intf; + info->data = intf; + + /* and a number of CDC descriptors */ + cdc_parse_cdc_header(&hdr, intf, buf, len); + cdc_union = hdr.usb_cdc_union_desc; + cdc_ether = hdr.usb_cdc_ether_desc; + + /* Use separate control and data interfaces if we found a CDC Union */ + if (cdc_union) { + info->data = usb_ifnum_to_if(dev->udev, + cdc_union->bSlaveInterface0); + if (desc->bInterfaceNumber != cdc_union->bMasterInterface0 || + !info->data) { + dev_err(&intf->dev, + "bogus CDC Union: master=%u, slave=%u\n", + cdc_union->bMasterInterface0, + cdc_union->bSlaveInterface0); + + /* ignore and continue... */ + cdc_union = NULL; + info->data = intf; + } + } + + /* errors aren't fatal - we can live with the dynamic address */ + if (cdc_ether && cdc_ether->wMaxSegmentSize) { + dev->hard_mtu = le16_to_cpu(cdc_ether->wMaxSegmentSize); + usbnet_get_ethernet_addr(dev, cdc_ether->iMACAddress); + } + + /* claim data interface and set it up */ + if (info->control != info->data) { + status = usb_driver_claim_interface(driver, info->data, dev); + if (status < 0) + goto err; + } + + status = qmi_wwan_register_subdriver(dev); + if (status < 0 && info->control != info->data) { + usb_set_intfdata(info->data, NULL); + usb_driver_release_interface(driver, info->data); + } + + /* disabling remote wakeup on MDM9x30 devices has the same + * effect as clearing DTR. The device will not respond to QMI + * requests until we set DTR again. This is similar to a + * QMI_CTL SYNC request, clearing a lot of firmware state + * including the client ID allocations. + * + * Our usage model allows a session to span multiple + * open/close events, so we must prevent the firmware from + * clearing out state the clients might need. + * + * MDM9x30 is the first QMI chipset with USB3 support. Abuse + * this fact to enable the quirk for all USB3 devices. + * + * There are also chipsets with the same "set DTR" requirement + * but without USB3 support. Devices based on these chips + * need a quirk flag in the device ID table. + */ + if (dev->driver_info->data & QMI_WWAN_QUIRK_DTR || + le16_to_cpu(dev->udev->descriptor.bcdUSB) >= 0x0201) { + qmi_wwan_manage_power(dev, 1); + qmi_wwan_change_dtr(dev, true); + } + + /* Never use the same address on both ends of the link, even if the + * buggy firmware told us to. Or, if device is assigned the well-known + * buggy firmware MAC address, replace it with a random address, + */ + if (ether_addr_equal(dev->net->dev_addr, default_modem_addr) || + ether_addr_equal(dev->net->dev_addr, buggy_fw_addr)) + eth_hw_addr_random(dev->net); + + /* make MAC addr easily distinguishable from an IP header */ + if (possibly_iphdr(dev->net->dev_addr)) { + dev->net->dev_addr[0] |= 0x02; /* set local assignment bit */ + dev->net->dev_addr[0] &= 0xbf; /* clear "IP" bit */ + } + dev->net->netdev_ops = &qmi_wwan_netdev_ops; + dev->net->sysfs_groups[0] = &qmi_wwan_sysfs_attr_group; + #if 1 /* Added by Quectel */ + if (dev->udev->descriptor.idVendor == cpu_to_le16(0x2C7C)) { + dev_info(&intf->dev, "Quectel EC21&EC25&EC20 R2.0 work on RawIP mode\n"); + dev->net->flags |= IFF_NOARP; + + usb_control_msg( + interface_to_usbdev(intf), + usb_sndctrlpipe(interface_to_usbdev(intf), 0), + 0x22, /* USB_CDC_REQ_SET_CONTROL_LINE_STATE */ + 0x21, /* USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE */ + 1, /* active CDC DTR */ + intf->cur_altsetting->desc.bInterfaceNumber, + NULL, 0, 100); + } + #endif +err: + return status; +} + +static void qmi_wwan_unbind(struct usbnet *dev, struct usb_interface *intf) +{ + struct qmi_wwan_state *info = (void *)&dev->data; + struct usb_driver *driver = driver_of(intf); + struct usb_interface *other; + + if (info->subdriver && info->subdriver->disconnect) + info->subdriver->disconnect(info->control); + + /* disable MDM9x30 quirk */ + if (le16_to_cpu(dev->udev->descriptor.bcdUSB) >= 0x0201) { + qmi_wwan_change_dtr(dev, false); + qmi_wwan_manage_power(dev, 0); + } + + /* allow user to unbind using either control or data */ + if (intf == info->control) + other = info->data; + else + other = info->control; + + /* only if not shared */ + if (other && intf != other) { + usb_set_intfdata(other, NULL); + usb_driver_release_interface(driver, other); + } + + info->subdriver = NULL; + info->data = NULL; + info->control = NULL; +} + +/* suspend/resume wrappers calling both usbnet and the cdc-wdm + * subdriver if present. + * + * NOTE: cdc-wdm also supports pre/post_reset, but we cannot provide + * wrappers for those without adding usbnet reset support first. + */ +static int qmi_wwan_suspend(struct usb_interface *intf, pm_message_t message) +{ + struct usbnet *dev = usb_get_intfdata(intf); + struct qmi_wwan_state *info = (void *)&dev->data; + int ret; + + /* Both usbnet_suspend() and subdriver->suspend() MUST return 0 + * in system sleep context, otherwise, the resume callback has + * to recover device from previous suspend failure. + */ + ret = usbnet_suspend(intf, message); + if (ret < 0) + goto err; + + if (intf == info->control && info->subdriver && + info->subdriver->suspend) + ret = info->subdriver->suspend(intf, message); + if (ret < 0) + usbnet_resume(intf); +err: + return ret; +} + +static int qmi_wwan_resume(struct usb_interface *intf) +{ + struct usbnet *dev = usb_get_intfdata(intf); + struct qmi_wwan_state *info = (void *)&dev->data; + int ret = 0; + bool callsub = (intf == info->control && info->subdriver && + info->subdriver->resume); + + if (callsub) + ret = info->subdriver->resume(intf); + if (ret < 0) + goto err; + ret = usbnet_resume(intf); + if (ret < 0 && callsub) + info->subdriver->suspend(intf, PMSG_SUSPEND); +err: + return ret; +} + +static const struct driver_info qmi_wwan_info = { + .description = "WWAN/QMI device", + .flags = FLAG_WWAN | FLAG_SEND_ZLP, + .bind = qmi_wwan_bind, + .unbind = qmi_wwan_unbind, + .manage_power = qmi_wwan_manage_power, + .rx_fixup = qmi_wwan_rx_fixup, + .tx_fixup = qmi_wwan_tx_fixup, +}; + +static const struct driver_info qmi_wwan_info_quirk_dtr = { + .description = "WWAN/QMI device", + .flags = FLAG_WWAN | FLAG_SEND_ZLP, + .bind = qmi_wwan_bind, + .unbind = qmi_wwan_unbind, + .manage_power = qmi_wwan_manage_power, + .rx_fixup = qmi_wwan_rx_fixup, + .data = QMI_WWAN_QUIRK_DTR, +}; + +#define HUAWEI_VENDOR_ID 0x12D1 + +/* map QMI/wwan function by a fixed interface number */ +#define QMI_FIXED_INTF(vend, prod, num) \ + USB_DEVICE_INTERFACE_NUMBER(vend, prod, num), \ + .driver_info = (unsigned long)&qmi_wwan_info + +/* devices requiring "set DTR" quirk */ +#define QMI_QUIRK_SET_DTR(vend, prod, num) \ + USB_DEVICE_INTERFACE_NUMBER(vend, prod, num), \ + .driver_info = (unsigned long)&qmi_wwan_info_quirk_dtr + +/* Gobi 1000 QMI/wwan interface number is 3 according to qcserial */ +#define QMI_GOBI1K_DEVICE(vend, prod) \ + QMI_FIXED_INTF(vend, prod, 3) + +/* Gobi 2000/3000 QMI/wwan interface number is 0 according to qcserial */ +#define QMI_GOBI_DEVICE(vend, prod) \ + QMI_FIXED_INTF(vend, prod, 0) + +static const struct usb_device_id products[] = { + /* 1. CDC ECM like devices match on the control interface */ + { /* Huawei E392, E398 and possibly others sharing both device id and more... */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 1, 9), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Vodafone/Huawei K5005 (12d1:14c8) and similar modems */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 1, 57), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* HUAWEI_INTERFACE_NDIS_CONTROL_QUALCOMM */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 0x01, 0x69), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Motorola Mapphone devices with MDM6600 */ + USB_VENDOR_AND_INTERFACE_INFO(0x22b8, USB_CLASS_VENDOR_SPEC, 0xfb, 0xff), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + + /* 2. Combined interface devices matching on class+protocol */ + { /* Huawei E367 and possibly others in "Windows mode" */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 1, 7), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Huawei E392, E398 and possibly others in "Windows mode" */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 1, 17), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* HUAWEI_NDIS_SINGLE_INTERFACE_VDF */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 0x01, 0x37), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* HUAWEI_INTERFACE_NDIS_HW_QUALCOMM */ + USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, USB_CLASS_VENDOR_SPEC, 0x01, 0x67), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Pantech UML290, P4200 and more */ + USB_VENDOR_AND_INTERFACE_INFO(0x106c, USB_CLASS_VENDOR_SPEC, 0xf0, 0xff), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Pantech UML290 - newer firmware */ + USB_VENDOR_AND_INTERFACE_INFO(0x106c, USB_CLASS_VENDOR_SPEC, 0xf1, 0xff), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Novatel USB551L and MC551 */ + USB_DEVICE_AND_INTERFACE_INFO(0x1410, 0xb001, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Novatel E362 */ + USB_DEVICE_AND_INTERFACE_INFO(0x1410, 0x9010, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Novatel Expedite E371 */ + USB_DEVICE_AND_INTERFACE_INFO(0x1410, 0x9011, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Dell Wireless 5800 (Novatel E362) */ + USB_DEVICE_AND_INTERFACE_INFO(0x413C, 0x8195, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Dell Wireless 5800 V2 (Novatel E362) */ + USB_DEVICE_AND_INTERFACE_INFO(0x413C, 0x8196, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* Dell Wireless 5804 (Novatel E371) */ + USB_DEVICE_AND_INTERFACE_INFO(0x413C, 0x819b, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* ADU960S */ + USB_DEVICE_AND_INTERFACE_INFO(0x16d5, 0x650a, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* HP lt2523 (Novatel E371) */ + USB_DEVICE_AND_INTERFACE_INFO(0x03f0, 0x421d, + USB_CLASS_COMM, + USB_CDC_SUBCLASS_ETHERNET, + USB_CDC_PROTO_NONE), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + { /* HP lt4112 LTE/HSPA+ Gobi 4G Module (Huawei me906e) */ + USB_DEVICE_AND_INTERFACE_INFO(0x03f0, 0x581d, USB_CLASS_VENDOR_SPEC, 1, 7), + .driver_info = (unsigned long)&qmi_wwan_info, + }, + + /* 3. Combined interface devices matching on interface number */ + {QMI_FIXED_INTF(0x0408, 0xea42, 4)}, /* Yota / Megafon M100-1 */ + {QMI_FIXED_INTF(0x05c6, 0x6001, 3)}, /* 4G LTE usb-modem U901 */ + {QMI_FIXED_INTF(0x05c6, 0x7000, 0)}, + {QMI_FIXED_INTF(0x05c6, 0x7001, 1)}, + {QMI_FIXED_INTF(0x05c6, 0x7002, 1)}, + {QMI_FIXED_INTF(0x05c6, 0x7101, 1)}, + {QMI_FIXED_INTF(0x05c6, 0x7101, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x7101, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x7102, 1)}, + {QMI_FIXED_INTF(0x05c6, 0x7102, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x7102, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x8000, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x8001, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9000, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9003, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9005, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x900a, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x900b, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x900c, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x900c, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x900c, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x900d, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x900f, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x900f, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x900f, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9010, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9010, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9011, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9011, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9021, 1)}, + {QMI_FIXED_INTF(0x05c6, 0x9022, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x9025, 4)}, /* Alcatel-sbell ASB TL131 TDD LTE (China Mobile) */ + {QMI_FIXED_INTF(0x05c6, 0x9026, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x902e, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9031, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9032, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9033, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9033, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9033, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9033, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9034, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9034, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9034, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9034, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9034, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9035, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9036, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9037, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9038, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x903b, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x903c, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x903d, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x903e, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9043, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9046, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9046, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9046, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9047, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x9047, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9047, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9048, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9048, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9048, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9048, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9048, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x904c, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x904c, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x904c, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x904c, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x9050, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9052, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9053, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9053, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9054, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9054, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9055, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9055, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9055, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9055, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9055, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9056, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x9062, 9)}, + {QMI_FIXED_INTF(0x05c6, 0x9064, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9065, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9065, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9066, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9066, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9067, 1)}, + {QMI_FIXED_INTF(0x05c6, 0x9068, 2)}, + {QMI_FIXED_INTF(0x05c6, 0x9068, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9068, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9068, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9068, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9068, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9069, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9069, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9069, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9069, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x9070, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9070, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9075, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9076, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9076, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9076, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9076, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9076, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x9077, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9077, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9077, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9077, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9078, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9079, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x9079, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9079, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9079, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9079, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x9080, 5)}, + {QMI_FIXED_INTF(0x05c6, 0x9080, 6)}, + {QMI_FIXED_INTF(0x05c6, 0x9080, 7)}, + {QMI_FIXED_INTF(0x05c6, 0x9080, 8)}, + {QMI_FIXED_INTF(0x05c6, 0x9083, 3)}, + {QMI_FIXED_INTF(0x05c6, 0x9084, 4)}, + {QMI_FIXED_INTF(0x05c6, 0x90b2, 3)}, /* ublox R410M */ + {QMI_FIXED_INTF(0x05c6, 0x920d, 0)}, + {QMI_FIXED_INTF(0x05c6, 0x920d, 5)}, + {QMI_QUIRK_SET_DTR(0x05c6, 0x9625, 4)}, /* YUGA CLM920-NC5 */ + {QMI_FIXED_INTF(0x0846, 0x68a2, 8)}, + {QMI_FIXED_INTF(0x0846, 0x68d3, 8)}, /* Netgear Aircard 779S */ + {QMI_FIXED_INTF(0x12d1, 0x140c, 1)}, /* Huawei E173 */ + {QMI_FIXED_INTF(0x12d1, 0x14ac, 1)}, /* Huawei E1820 */ + {QMI_FIXED_INTF(0x1435, 0xd181, 3)}, /* Wistron NeWeb D18Q1 */ + {QMI_FIXED_INTF(0x1435, 0xd181, 4)}, /* Wistron NeWeb D18Q1 */ + {QMI_FIXED_INTF(0x1435, 0xd181, 5)}, /* Wistron NeWeb D18Q1 */ + {QMI_FIXED_INTF(0x16d8, 0x6003, 0)}, /* CMOTech 6003 */ + {QMI_FIXED_INTF(0x16d8, 0x6007, 0)}, /* CMOTech CHE-628S */ + {QMI_FIXED_INTF(0x16d8, 0x6008, 0)}, /* CMOTech CMU-301 */ + {QMI_FIXED_INTF(0x16d8, 0x6280, 0)}, /* CMOTech CHU-628 */ + {QMI_FIXED_INTF(0x16d8, 0x7001, 0)}, /* CMOTech CHU-720S */ + {QMI_FIXED_INTF(0x16d8, 0x7002, 0)}, /* CMOTech 7002 */ + {QMI_FIXED_INTF(0x16d8, 0x7003, 4)}, /* CMOTech CHU-629K */ + {QMI_FIXED_INTF(0x16d8, 0x7004, 3)}, /* CMOTech 7004 */ + {QMI_FIXED_INTF(0x16d8, 0x7006, 5)}, /* CMOTech CGU-629 */ + {QMI_FIXED_INTF(0x16d8, 0x700a, 4)}, /* CMOTech CHU-629S */ + {QMI_FIXED_INTF(0x16d8, 0x7211, 0)}, /* CMOTech CHU-720I */ + {QMI_FIXED_INTF(0x16d8, 0x7212, 0)}, /* CMOTech 7212 */ + {QMI_FIXED_INTF(0x16d8, 0x7213, 0)}, /* CMOTech 7213 */ + {QMI_FIXED_INTF(0x16d8, 0x7251, 1)}, /* CMOTech 7251 */ + {QMI_FIXED_INTF(0x16d8, 0x7252, 1)}, /* CMOTech 7252 */ + {QMI_FIXED_INTF(0x16d8, 0x7253, 1)}, /* CMOTech 7253 */ + {QMI_FIXED_INTF(0x19d2, 0x0002, 1)}, + {QMI_FIXED_INTF(0x19d2, 0x0012, 1)}, + {QMI_FIXED_INTF(0x19d2, 0x0017, 3)}, + {QMI_FIXED_INTF(0x19d2, 0x0019, 3)}, /* ONDA MT689DC */ + {QMI_FIXED_INTF(0x19d2, 0x0021, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0025, 1)}, + {QMI_FIXED_INTF(0x19d2, 0x0031, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0042, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0049, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0052, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0055, 1)}, /* ZTE (Vodafone) K3520-Z */ + {QMI_FIXED_INTF(0x19d2, 0x0058, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0063, 4)}, /* ZTE (Vodafone) K3565-Z */ + {QMI_FIXED_INTF(0x19d2, 0x0104, 4)}, /* ZTE (Vodafone) K4505-Z */ + {QMI_FIXED_INTF(0x19d2, 0x0113, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0118, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0121, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0123, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0124, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0125, 6)}, + {QMI_FIXED_INTF(0x19d2, 0x0126, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0130, 1)}, + {QMI_FIXED_INTF(0x19d2, 0x0133, 3)}, + {QMI_FIXED_INTF(0x19d2, 0x0141, 5)}, + {QMI_FIXED_INTF(0x19d2, 0x0157, 5)}, /* ZTE MF683 */ + {QMI_FIXED_INTF(0x19d2, 0x0158, 3)}, + {QMI_FIXED_INTF(0x19d2, 0x0167, 4)}, /* ZTE MF820D */ + {QMI_FIXED_INTF(0x19d2, 0x0168, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x0176, 3)}, + {QMI_FIXED_INTF(0x19d2, 0x0178, 3)}, + {QMI_FIXED_INTF(0x19d2, 0x0191, 4)}, /* ZTE EuFi890 */ + {QMI_FIXED_INTF(0x19d2, 0x0199, 1)}, /* ZTE MF820S */ + {QMI_FIXED_INTF(0x19d2, 0x0200, 1)}, + {QMI_FIXED_INTF(0x19d2, 0x0257, 3)}, /* ZTE MF821 */ + {QMI_FIXED_INTF(0x19d2, 0x0265, 4)}, /* ONDA MT8205 4G LTE */ + {QMI_FIXED_INTF(0x19d2, 0x0284, 4)}, /* ZTE MF880 */ + {QMI_FIXED_INTF(0x19d2, 0x0326, 4)}, /* ZTE MF821D */ + {QMI_FIXED_INTF(0x19d2, 0x0412, 4)}, /* Telewell TW-LTE 4G */ + {QMI_FIXED_INTF(0x19d2, 0x1008, 4)}, /* ZTE (Vodafone) K3570-Z */ + {QMI_FIXED_INTF(0x19d2, 0x1010, 4)}, /* ZTE (Vodafone) K3571-Z */ + {QMI_FIXED_INTF(0x19d2, 0x1012, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1018, 3)}, /* ZTE (Vodafone) K5006-Z */ + {QMI_FIXED_INTF(0x19d2, 0x1021, 2)}, + {QMI_FIXED_INTF(0x19d2, 0x1245, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1247, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1252, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1254, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1255, 3)}, + {QMI_FIXED_INTF(0x19d2, 0x1255, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1256, 4)}, + {QMI_FIXED_INTF(0x19d2, 0x1270, 5)}, /* ZTE MF667 */ + {QMI_FIXED_INTF(0x19d2, 0x1401, 2)}, + {QMI_FIXED_INTF(0x19d2, 0x1402, 2)}, /* ZTE MF60 */ + {QMI_FIXED_INTF(0x19d2, 0x1424, 2)}, + {QMI_FIXED_INTF(0x19d2, 0x1425, 2)}, + {QMI_FIXED_INTF(0x19d2, 0x1426, 2)}, /* ZTE MF91 */ + {QMI_FIXED_INTF(0x19d2, 0x1428, 2)}, /* Telewell TW-LTE 4G v2 */ + {QMI_FIXED_INTF(0x19d2, 0x2002, 4)}, /* ZTE (Vodafone) K3765-Z */ + {QMI_FIXED_INTF(0x2001, 0x7e19, 4)}, /* D-Link DWM-221 B1 */ + {QMI_FIXED_INTF(0x2001, 0x7e35, 4)}, /* D-Link DWM-222 */ + {QMI_FIXED_INTF(0x2020, 0x2033, 4)}, /* BroadMobi BM806U */ + {QMI_FIXED_INTF(0x0f3d, 0x68a2, 8)}, /* Sierra Wireless MC7700 */ + {QMI_FIXED_INTF(0x114f, 0x68a2, 8)}, /* Sierra Wireless MC7750 */ + {QMI_FIXED_INTF(0x1199, 0x68a2, 8)}, /* Sierra Wireless MC7710 in QMI mode */ + {QMI_FIXED_INTF(0x1199, 0x68a2, 19)}, /* Sierra Wireless MC7710 in QMI mode */ + {QMI_FIXED_INTF(0x1199, 0x68c0, 8)}, /* Sierra Wireless MC7304/MC7354 */ + {QMI_FIXED_INTF(0x1199, 0x68c0, 10)}, /* Sierra Wireless MC7304/MC7354 */ + {QMI_FIXED_INTF(0x1199, 0x901c, 8)}, /* Sierra Wireless EM7700 */ + {QMI_FIXED_INTF(0x1199, 0x901f, 8)}, /* Sierra Wireless EM7355 */ + {QMI_FIXED_INTF(0x1199, 0x9041, 8)}, /* Sierra Wireless MC7305/MC7355 */ + {QMI_FIXED_INTF(0x1199, 0x9041, 10)}, /* Sierra Wireless MC7305/MC7355 */ + {QMI_FIXED_INTF(0x1199, 0x9051, 8)}, /* Netgear AirCard 340U */ + {QMI_FIXED_INTF(0x1199, 0x9053, 8)}, /* Sierra Wireless Modem */ + {QMI_FIXED_INTF(0x1199, 0x9054, 8)}, /* Sierra Wireless Modem */ + {QMI_FIXED_INTF(0x1199, 0x9055, 8)}, /* Netgear AirCard 341U */ + {QMI_FIXED_INTF(0x1199, 0x9056, 8)}, /* Sierra Wireless Modem */ + {QMI_FIXED_INTF(0x1199, 0x9057, 8)}, + {QMI_FIXED_INTF(0x1199, 0x9061, 8)}, /* Sierra Wireless Modem */ + {QMI_FIXED_INTF(0x1199, 0x9071, 8)}, /* Sierra Wireless MC74xx */ + {QMI_FIXED_INTF(0x1199, 0x9071, 10)}, /* Sierra Wireless MC74xx */ + {QMI_FIXED_INTF(0x1199, 0x9079, 8)}, /* Sierra Wireless EM74xx */ + {QMI_FIXED_INTF(0x1199, 0x9079, 10)}, /* Sierra Wireless EM74xx */ + {QMI_FIXED_INTF(0x1199, 0x907b, 8)}, /* Sierra Wireless EM74xx */ + {QMI_FIXED_INTF(0x1199, 0x907b, 10)}, /* Sierra Wireless EM74xx */ + {QMI_FIXED_INTF(0x1199, 0x9091, 8)}, /* Sierra Wireless EM7565 */ + {QMI_FIXED_INTF(0x1bbb, 0x011e, 4)}, /* Telekom Speedstick LTE II (Alcatel One Touch L100V LTE) */ + {QMI_FIXED_INTF(0x1bbb, 0x0203, 2)}, /* Alcatel L800MA */ + {QMI_FIXED_INTF(0x2357, 0x0201, 4)}, /* TP-LINK HSUPA Modem MA180 */ + {QMI_FIXED_INTF(0x2357, 0x9000, 4)}, /* TP-LINK MA260 */ + {QMI_QUIRK_SET_DTR(0x1bc7, 0x1040, 2)}, /* Telit LE922A */ + {QMI_FIXED_INTF(0x1bc7, 0x1100, 3)}, /* Telit ME910 */ + {QMI_FIXED_INTF(0x1bc7, 0x1101, 3)}, /* Telit ME910 dual modem */ + {QMI_FIXED_INTF(0x1bc7, 0x1200, 5)}, /* Telit LE920 */ + {QMI_FIXED_INTF(0x1bc7, 0x1201, 2)}, /* Telit LE920 */ + {QMI_FIXED_INTF(0x1c9e, 0x9b01, 3)}, /* XS Stick W100-2 from 4G Systems */ + {QMI_FIXED_INTF(0x0b3c, 0xc000, 4)}, /* Olivetti Olicard 100 */ + {QMI_FIXED_INTF(0x0b3c, 0xc001, 4)}, /* Olivetti Olicard 120 */ + {QMI_FIXED_INTF(0x0b3c, 0xc002, 4)}, /* Olivetti Olicard 140 */ + {QMI_FIXED_INTF(0x0b3c, 0xc004, 6)}, /* Olivetti Olicard 155 */ + {QMI_FIXED_INTF(0x0b3c, 0xc005, 6)}, /* Olivetti Olicard 200 */ + {QMI_FIXED_INTF(0x0b3c, 0xc00a, 6)}, /* Olivetti Olicard 160 */ + {QMI_FIXED_INTF(0x0b3c, 0xc00b, 4)}, /* Olivetti Olicard 500 */ + {QMI_FIXED_INTF(0x1e2d, 0x0060, 4)}, /* Cinterion PLxx */ + {QMI_FIXED_INTF(0x1e2d, 0x0053, 4)}, /* Cinterion PHxx,PXxx */ + {QMI_FIXED_INTF(0x1e2d, 0x0063, 10)}, /* Cinterion ALASxx (1 RmNet) */ + {QMI_FIXED_INTF(0x1e2d, 0x0082, 4)}, /* Cinterion PHxx,PXxx (2 RmNet) */ + {QMI_FIXED_INTF(0x1e2d, 0x0082, 5)}, /* Cinterion PHxx,PXxx (2 RmNet) */ + {QMI_FIXED_INTF(0x1e2d, 0x0083, 4)}, /* Cinterion PHxx,PXxx (1 RmNet + USB Audio)*/ + {QMI_FIXED_INTF(0x413c, 0x81a2, 8)}, /* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card */ + {QMI_FIXED_INTF(0x413c, 0x81a3, 8)}, /* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card */ + {QMI_FIXED_INTF(0x413c, 0x81a4, 8)}, /* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card */ + {QMI_FIXED_INTF(0x413c, 0x81a8, 8)}, /* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */ + {QMI_FIXED_INTF(0x413c, 0x81a9, 8)}, /* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */ + {QMI_FIXED_INTF(0x413c, 0x81b1, 8)}, /* Dell Wireless 5809e Gobi(TM) 4G LTE Mobile Broadband Card */ + {QMI_FIXED_INTF(0x413c, 0x81b3, 8)}, /* Dell Wireless 5809e Gobi(TM) 4G LTE Mobile Broadband Card (rev3) */ + {QMI_FIXED_INTF(0x413c, 0x81b6, 8)}, /* Dell Wireless 5811e */ + {QMI_FIXED_INTF(0x413c, 0x81b6, 10)}, /* Dell Wireless 5811e */ + {QMI_FIXED_INTF(0x413c, 0x81d7, 0)}, /* Dell Wireless 5821e */ + {QMI_FIXED_INTF(0x03f0, 0x4e1d, 8)}, /* HP lt4111 LTE/EV-DO/HSPA+ Gobi 4G Module */ + {QMI_FIXED_INTF(0x03f0, 0x9d1d, 1)}, /* HP lt4120 Snapdragon X5 LTE */ + {QMI_FIXED_INTF(0x22de, 0x9061, 3)}, /* WeTelecom WPD-600N */ + {QMI_FIXED_INTF(0x1e0e, 0x9001, 5)}, /* SIMCom 7230E */ + {QMI_QUIRK_SET_DTR(0x2c7c, 0x0125, 4)}, /* Quectel EC25, EC20 R2.0 Mini PCIe */ + {QMI_QUIRK_SET_DTR(0x2c7c, 0x0121, 4)}, /* Quectel EC21 Mini PCIe */ + {QMI_QUIRK_SET_DTR(0x2c7c, 0x0191, 4)}, /* Quectel EG91 */ + {QMI_FIXED_INTF(0x2c7c, 0x0296, 4)}, /* Quectel BG96 */ + {QMI_QUIRK_SET_DTR(0x2c7c, 0x0306, 4)}, /* Quectel EP06 Mini PCIe */ + + /* 4. Gobi 1000 devices */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9212)}, /* Acer Gobi Modem Device */ + {QMI_GOBI1K_DEVICE(0x03f0, 0x1f1d)}, /* HP un2400 Gobi Modem Device */ + {QMI_GOBI1K_DEVICE(0x04da, 0x250d)}, /* Panasonic Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x413c, 0x8172)}, /* Dell Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa001)}, /* Novatel/Verizon USB-1000 */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa002)}, /* Novatel Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa003)}, /* Novatel Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa004)}, /* Novatel Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa005)}, /* Novatel Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa006)}, /* Novatel Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x1410, 0xa007)}, /* Novatel Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x0b05, 0x1776)}, /* Asus Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x19d2, 0xfff3)}, /* ONDA Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9001)}, /* Generic Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9002)}, /* Generic Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9202)}, /* Generic Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9203)}, /* Generic Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9222)}, /* Generic Gobi Modem device */ + {QMI_GOBI1K_DEVICE(0x05c6, 0x9009)}, /* Generic Gobi Modem device */ + + /* 5. Gobi 2000 and 3000 devices */ + {QMI_GOBI_DEVICE(0x413c, 0x8186)}, /* Dell Gobi 2000 Modem device (N0218, VU936) */ + {QMI_GOBI_DEVICE(0x413c, 0x8194)}, /* Dell Gobi 3000 Composite */ + {QMI_GOBI_DEVICE(0x05c6, 0x920b)}, /* Generic Gobi 2000 Modem device */ + {QMI_GOBI_DEVICE(0x05c6, 0x9225)}, /* Sony Gobi 2000 Modem device (N0279, VU730) */ + {QMI_GOBI_DEVICE(0x05c6, 0x9245)}, /* Samsung Gobi 2000 Modem device (VL176) */ + {QMI_GOBI_DEVICE(0x03f0, 0x251d)}, /* HP Gobi 2000 Modem device (VP412) */ + {QMI_GOBI_DEVICE(0x05c6, 0x9215)}, /* Acer Gobi 2000 Modem device (VP413) */ + {QMI_FIXED_INTF(0x05c6, 0x9215, 4)}, /* Quectel EC20 Mini PCIe */ + {QMI_GOBI_DEVICE(0x05c6, 0x9265)}, /* Asus Gobi 2000 Modem device (VR305) */ + {QMI_GOBI_DEVICE(0x05c6, 0x9235)}, /* Top Global Gobi 2000 Modem device (VR306) */ + {QMI_GOBI_DEVICE(0x05c6, 0x9275)}, /* iRex Technologies Gobi 2000 Modem device (VR307) */ + {QMI_GOBI_DEVICE(0x0af0, 0x8120)}, /* Option GTM681W */ + {QMI_GOBI_DEVICE(0x1199, 0x68a5)}, /* Sierra Wireless Modem */ + {QMI_GOBI_DEVICE(0x1199, 0x68a9)}, /* Sierra Wireless Modem */ + {QMI_GOBI_DEVICE(0x1199, 0x9001)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9002)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9003)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9004)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9005)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9006)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9007)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9008)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9009)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x900a)}, /* Sierra Wireless Gobi 2000 Modem device (VT773) */ + {QMI_GOBI_DEVICE(0x1199, 0x9011)}, /* Sierra Wireless Gobi 2000 Modem device (MC8305) */ + {QMI_GOBI_DEVICE(0x16d8, 0x8002)}, /* CMDTech Gobi 2000 Modem device (VU922) */ + {QMI_GOBI_DEVICE(0x05c6, 0x9205)}, /* Gobi 2000 Modem device */ + {QMI_GOBI_DEVICE(0x1199, 0x9013)}, /* Sierra Wireless Gobi 3000 Modem device (MC8355) */ + {QMI_GOBI_DEVICE(0x03f0, 0x371d)}, /* HP un2430 Mobile Broadband Module */ + {QMI_GOBI_DEVICE(0x1199, 0x9015)}, /* Sierra Wireless Gobi 3000 Modem device */ + {QMI_GOBI_DEVICE(0x1199, 0x9019)}, /* Sierra Wireless Gobi 3000 Modem device */ + {QMI_GOBI_DEVICE(0x1199, 0x901b)}, /* Sierra Wireless MC7770 */ + {QMI_GOBI_DEVICE(0x12d1, 0x14f1)}, /* Sony Gobi 3000 Composite */ + {QMI_GOBI_DEVICE(0x1410, 0xa021)}, /* Foxconn Gobi 3000 Modem device (Novatel E396) */ + #if 1 + //Added by Quectel + #ifndef QMI_FIXED_INTF + /* map QMI/wwan function by a fixed interface number */ + #define QMI_FIXED_INTF(vend, prod, num) \ + .match_flags = USB_DEVICE_ID_MATCH_DEVICE | USB_DEVICE_ID_MATCH_INT_INFO, \ + .idVendor = vend, \ + .idProduct = prod, \ + .bInterfaceClass = 0xff, \ + .bInterfaceSubClass = 0xff, \ + .bInterfaceProtocol = 0xff, \ + .driver_info= (unsigned long)&qmi_wwan_force_int##num, + #endif + { QMI_FIXED_INTF(0x05C6, 0x9003, 4) }, /* Quectel UC20 */ + { QMI_FIXED_INTF(0x05C6, 0x9215, 4) }, /* Quectel EC20 */ + { QMI_FIXED_INTF(0x2C7C, 0x0125, 4) }, /* Quectel EC25/EC20 R2.0 */ + { QMI_FIXED_INTF(0x2C7C, 0x0121, 4) }, /* Quectel EC21 */ + #endif + { } /* END */ +}; +MODULE_DEVICE_TABLE(usb, products); + +static bool quectel_ec20_detected(struct usb_interface *intf) +{ + struct usb_device *dev = interface_to_usbdev(intf); + + if (dev->actconfig && + le16_to_cpu(dev->descriptor.idVendor) == 0x05c6 && + le16_to_cpu(dev->descriptor.idProduct) == 0x9215 && + dev->actconfig->desc.bNumInterfaces == 5) + return true; + + return false; +} + +static int qmi_wwan_probe(struct usb_interface *intf, + const struct usb_device_id *prod) +{ + struct usb_device_id *id = (struct usb_device_id *)prod; + struct usb_interface_descriptor *desc = &intf->cur_altsetting->desc; + + /* Workaround to enable dynamic IDs. This disables usbnet + * blacklisting functionality. Which, if required, can be + * reimplemented here by using a magic "blacklist" value + * instead of 0 in the static device id table + */ + if (!id->driver_info) { + dev_dbg(&intf->dev, "setting defaults for dynamic device id\n"); + id->driver_info = (unsigned long)&qmi_wwan_info; + } + + /* There are devices where the same interface number can be + * configured as different functions. We should only bind to + * vendor specific functions when matching on interface number + */ + if (id->match_flags & USB_DEVICE_ID_MATCH_INT_NUMBER && + desc->bInterfaceClass != USB_CLASS_VENDOR_SPEC) { + dev_dbg(&intf->dev, + "Rejecting interface number match for class %02x\n", + desc->bInterfaceClass); + return -ENODEV; + } + + /* Quectel EC20 quirk where we've QMI on interface 4 instead of 0 */ + if (quectel_ec20_detected(intf) && desc->bInterfaceNumber == 0) { + dev_dbg(&intf->dev, "Quectel EC20 quirk, skipping interface 0\n"); + return -ENODEV; + } + + return usbnet_probe(intf, id); +} + +static struct usb_driver qmi_wwan_driver = { + .name = "qmi_wwan", + .id_table = products, + .probe = qmi_wwan_probe, + .disconnect = usbnet_disconnect, + .suspend = qmi_wwan_suspend, + .resume = qmi_wwan_resume, + .reset_resume = qmi_wwan_resume, + .supports_autosuspend = 1, + .disable_hub_initiated_lpm = 1, +}; + +module_usb_driver(qmi_wwan_driver); + +MODULE_AUTHOR("Bjørn Mork "); +MODULE_DESCRIPTION("Qualcomm MSM Interface (QMI) WWAN driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c new file mode 100644 index 0000000..5e1e082 --- /dev/null +++ b/drivers/usb/serial/option.c @@ -0,0 +1,2127 @@ +/* + USB Driver for GSM modems + + Copyright (C) 2005 Matthias Urlichs + + This driver is free software; you can redistribute it and/or modify + it under the terms of Version 2 of the GNU General Public License as + published by the Free Software Foundation. + + Portions copied from the Keyspan driver by Hugh Blemings + + History: see the git log. + + Work sponsored by: Sigos GmbH, Germany + + This driver exists because the "normal" serial driver doesn't work too well + with GSM modems. Issues: + - data loss -- one single Receive URB is not nearly enough + - nonstandard flow (Option devices) control + - controlling the baud rate doesn't make sense + + This driver is named "option" because the most common device it's + used for is a PC-Card (with an internal OHCI-USB interface, behind + which the GSM interface sits), made by Option Inc. + + Some of the "one port" devices actually exhibit multiple USB instances + on the USB bus. This is not a bug, these ports are used for different + device features. +*/ + +#define DRIVER_AUTHOR "Matthias Urlichs " +#define DRIVER_DESC "USB Driver for GSM modems" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "usb-wwan.h" + +/* Function prototypes */ +static int option_probe(struct usb_serial *serial, + const struct usb_device_id *id); +static int option_attach(struct usb_serial *serial); +static void option_release(struct usb_serial *serial); +static void option_instat_callback(struct urb *urb); + +/* Vendor and product IDs */ +#define OPTION_VENDOR_ID 0x0AF0 +#define OPTION_PRODUCT_COLT 0x5000 +#define OPTION_PRODUCT_RICOLA 0x6000 +#define OPTION_PRODUCT_RICOLA_LIGHT 0x6100 +#define OPTION_PRODUCT_RICOLA_QUAD 0x6200 +#define OPTION_PRODUCT_RICOLA_QUAD_LIGHT 0x6300 +#define OPTION_PRODUCT_RICOLA_NDIS 0x6050 +#define OPTION_PRODUCT_RICOLA_NDIS_LIGHT 0x6150 +#define OPTION_PRODUCT_RICOLA_NDIS_QUAD 0x6250 +#define OPTION_PRODUCT_RICOLA_NDIS_QUAD_LIGHT 0x6350 +#define OPTION_PRODUCT_COBRA 0x6500 +#define OPTION_PRODUCT_COBRA_BUS 0x6501 +#define OPTION_PRODUCT_VIPER 0x6600 +#define OPTION_PRODUCT_VIPER_BUS 0x6601 +#define OPTION_PRODUCT_GT_MAX_READY 0x6701 +#define OPTION_PRODUCT_FUJI_MODEM_LIGHT 0x6721 +#define OPTION_PRODUCT_FUJI_MODEM_GT 0x6741 +#define OPTION_PRODUCT_FUJI_MODEM_EX 0x6761 +#define OPTION_PRODUCT_KOI_MODEM 0x6800 +#define OPTION_PRODUCT_SCORPION_MODEM 0x6901 +#define OPTION_PRODUCT_ETNA_MODEM 0x7001 +#define OPTION_PRODUCT_ETNA_MODEM_LITE 0x7021 +#define OPTION_PRODUCT_ETNA_MODEM_GT 0x7041 +#define OPTION_PRODUCT_ETNA_MODEM_EX 0x7061 +#define OPTION_PRODUCT_ETNA_KOI_MODEM 0x7100 +#define OPTION_PRODUCT_GTM380_MODEM 0x7201 + +#define HUAWEI_VENDOR_ID 0x12D1 +#define HUAWEI_PRODUCT_E173 0x140C +#define HUAWEI_PRODUCT_E1750 0x1406 +#define HUAWEI_PRODUCT_K4505 0x1464 +#define HUAWEI_PRODUCT_K3765 0x1465 +#define HUAWEI_PRODUCT_K4605 0x14C6 +#define HUAWEI_PRODUCT_E173S6 0x1C07 + +#define QUANTA_VENDOR_ID 0x0408 +#define QUANTA_PRODUCT_Q101 0xEA02 +#define QUANTA_PRODUCT_Q111 0xEA03 +#define QUANTA_PRODUCT_GLX 0xEA04 +#define QUANTA_PRODUCT_GKE 0xEA05 +#define QUANTA_PRODUCT_GLE 0xEA06 + +#define NOVATELWIRELESS_VENDOR_ID 0x1410 + +/* YISO PRODUCTS */ + +#define YISO_VENDOR_ID 0x0EAB +#define YISO_PRODUCT_U893 0xC893 + +/* + * NOVATEL WIRELESS PRODUCTS + * + * Note from Novatel Wireless: + * If your Novatel modem does not work on linux, don't + * change the option module, but check our website. If + * that does not help, contact ddeschepper@nvtl.com +*/ +/* MERLIN EVDO PRODUCTS */ +#define NOVATELWIRELESS_PRODUCT_V640 0x1100 +#define NOVATELWIRELESS_PRODUCT_V620 0x1110 +#define NOVATELWIRELESS_PRODUCT_V740 0x1120 +#define NOVATELWIRELESS_PRODUCT_V720 0x1130 + +/* MERLIN HSDPA/HSPA PRODUCTS */ +#define NOVATELWIRELESS_PRODUCT_U730 0x1400 +#define NOVATELWIRELESS_PRODUCT_U740 0x1410 +#define NOVATELWIRELESS_PRODUCT_U870 0x1420 +#define NOVATELWIRELESS_PRODUCT_XU870 0x1430 +#define NOVATELWIRELESS_PRODUCT_X950D 0x1450 + +/* EXPEDITE PRODUCTS */ +#define NOVATELWIRELESS_PRODUCT_EV620 0x2100 +#define NOVATELWIRELESS_PRODUCT_ES720 0x2110 +#define NOVATELWIRELESS_PRODUCT_E725 0x2120 +#define NOVATELWIRELESS_PRODUCT_ES620 0x2130 +#define NOVATELWIRELESS_PRODUCT_EU730 0x2400 +#define NOVATELWIRELESS_PRODUCT_EU740 0x2410 +#define NOVATELWIRELESS_PRODUCT_EU870D 0x2420 +/* OVATION PRODUCTS */ +#define NOVATELWIRELESS_PRODUCT_MC727 0x4100 +#define NOVATELWIRELESS_PRODUCT_MC950D 0x4400 +/* + * Note from Novatel Wireless: + * All PID in the 5xxx range are currently reserved for + * auto-install CDROMs, and should not be added to this + * module. + * + * #define NOVATELWIRELESS_PRODUCT_U727 0x5010 + * #define NOVATELWIRELESS_PRODUCT_MC727_NEW 0x5100 +*/ +#define NOVATELWIRELESS_PRODUCT_OVMC760 0x6002 +#define NOVATELWIRELESS_PRODUCT_MC780 0x6010 +#define NOVATELWIRELESS_PRODUCT_EVDO_FULLSPEED 0x6000 +#define NOVATELWIRELESS_PRODUCT_EVDO_HIGHSPEED 0x6001 +#define NOVATELWIRELESS_PRODUCT_HSPA_FULLSPEED 0x7000 +#define NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED 0x7001 +#define NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED3 0x7003 +#define NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED4 0x7004 +#define NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED5 0x7005 +#define NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED6 0x7006 +#define NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED7 0x7007 +#define NOVATELWIRELESS_PRODUCT_MC996D 0x7030 +#define NOVATELWIRELESS_PRODUCT_MF3470 0x7041 +#define NOVATELWIRELESS_PRODUCT_MC547 0x7042 +#define NOVATELWIRELESS_PRODUCT_EVDO_EMBEDDED_FULLSPEED 0x8000 +#define NOVATELWIRELESS_PRODUCT_EVDO_EMBEDDED_HIGHSPEED 0x8001 +#define NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_FULLSPEED 0x9000 +#define NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_HIGHSPEED 0x9001 +#define NOVATELWIRELESS_PRODUCT_E362 0x9010 +#define NOVATELWIRELESS_PRODUCT_E371 0x9011 +#define NOVATELWIRELESS_PRODUCT_U620L 0x9022 +#define NOVATELWIRELESS_PRODUCT_G2 0xA010 +#define NOVATELWIRELESS_PRODUCT_MC551 0xB001 + +/* AMOI PRODUCTS */ +#define AMOI_VENDOR_ID 0x1614 +#define AMOI_PRODUCT_H01 0x0800 +#define AMOI_PRODUCT_H01A 0x7002 +#define AMOI_PRODUCT_H02 0x0802 +#define AMOI_PRODUCT_SKYPEPHONE_S2 0x0407 + +#define DELL_VENDOR_ID 0x413C + +/* Dell modems */ +#define DELL_PRODUCT_5700_MINICARD 0x8114 +#define DELL_PRODUCT_5500_MINICARD 0x8115 +#define DELL_PRODUCT_5505_MINICARD 0x8116 +#define DELL_PRODUCT_5700_EXPRESSCARD 0x8117 +#define DELL_PRODUCT_5510_EXPRESSCARD 0x8118 + +#define DELL_PRODUCT_5700_MINICARD_SPRINT 0x8128 +#define DELL_PRODUCT_5700_MINICARD_TELUS 0x8129 + +#define DELL_PRODUCT_5720_MINICARD_VZW 0x8133 +#define DELL_PRODUCT_5720_MINICARD_SPRINT 0x8134 +#define DELL_PRODUCT_5720_MINICARD_TELUS 0x8135 +#define DELL_PRODUCT_5520_MINICARD_CINGULAR 0x8136 +#define DELL_PRODUCT_5520_MINICARD_GENERIC_L 0x8137 +#define DELL_PRODUCT_5520_MINICARD_GENERIC_I 0x8138 + +#define DELL_PRODUCT_5730_MINICARD_SPRINT 0x8180 +#define DELL_PRODUCT_5730_MINICARD_TELUS 0x8181 +#define DELL_PRODUCT_5730_MINICARD_VZW 0x8182 + +#define DELL_PRODUCT_5800_MINICARD_VZW 0x8195 /* Novatel E362 */ +#define DELL_PRODUCT_5800_V2_MINICARD_VZW 0x8196 /* Novatel E362 */ +#define DELL_PRODUCT_5804_MINICARD_ATT 0x819b /* Novatel E371 */ + +#define DELL_PRODUCT_5821E 0x81d7 + +#define KYOCERA_VENDOR_ID 0x0c88 +#define KYOCERA_PRODUCT_KPC650 0x17da +#define KYOCERA_PRODUCT_KPC680 0x180a + +#define ANYDATA_VENDOR_ID 0x16d5 +#define ANYDATA_PRODUCT_ADU_620UW 0x6202 +#define ANYDATA_PRODUCT_ADU_E100A 0x6501 +#define ANYDATA_PRODUCT_ADU_500A 0x6502 + +#define AXESSTEL_VENDOR_ID 0x1726 +#define AXESSTEL_PRODUCT_MV110H 0x1000 + +#define BANDRICH_VENDOR_ID 0x1A8D +#define BANDRICH_PRODUCT_C100_1 0x1002 +#define BANDRICH_PRODUCT_C100_2 0x1003 +#define BANDRICH_PRODUCT_1004 0x1004 +#define BANDRICH_PRODUCT_1005 0x1005 +#define BANDRICH_PRODUCT_1006 0x1006 +#define BANDRICH_PRODUCT_1007 0x1007 +#define BANDRICH_PRODUCT_1008 0x1008 +#define BANDRICH_PRODUCT_1009 0x1009 +#define BANDRICH_PRODUCT_100A 0x100a + +#define BANDRICH_PRODUCT_100B 0x100b +#define BANDRICH_PRODUCT_100C 0x100c +#define BANDRICH_PRODUCT_100D 0x100d +#define BANDRICH_PRODUCT_100E 0x100e + +#define BANDRICH_PRODUCT_100F 0x100f +#define BANDRICH_PRODUCT_1010 0x1010 +#define BANDRICH_PRODUCT_1011 0x1011 +#define BANDRICH_PRODUCT_1012 0x1012 + +#define QUALCOMM_VENDOR_ID 0x05C6 +/* These Quectel products use Qualcomm's vendor ID */ +#define QUECTEL_PRODUCT_UC20 0x9003 +#define QUECTEL_PRODUCT_UC15 0x9090 +/* These u-blox products use Qualcomm's vendor ID */ +#define UBLOX_PRODUCT_R410M 0x90b2 +/* These Yuga products use Qualcomm's vendor ID */ +#define YUGA_PRODUCT_CLM920_NC5 0x9625 + +#define QUECTEL_VENDOR_ID 0x2c7c +/* These Quectel products use Quectel's vendor ID */ +#define QUECTEL_PRODUCT_EC21 0x0121 +#define QUECTEL_PRODUCT_EC25 0x0125 +#define QUECTEL_PRODUCT_BG96 0x0296 +#define QUECTEL_PRODUCT_EP06 0x0306 + +#define CMOTECH_VENDOR_ID 0x16d8 +#define CMOTECH_PRODUCT_6001 0x6001 +#define CMOTECH_PRODUCT_CMU_300 0x6002 +#define CMOTECH_PRODUCT_6003 0x6003 +#define CMOTECH_PRODUCT_6004 0x6004 +#define CMOTECH_PRODUCT_6005 0x6005 +#define CMOTECH_PRODUCT_CGU_628A 0x6006 +#define CMOTECH_PRODUCT_CHE_628S 0x6007 +#define CMOTECH_PRODUCT_CMU_301 0x6008 +#define CMOTECH_PRODUCT_CHU_628 0x6280 +#define CMOTECH_PRODUCT_CHU_628S 0x6281 +#define CMOTECH_PRODUCT_CDU_680 0x6803 +#define CMOTECH_PRODUCT_CDU_685A 0x6804 +#define CMOTECH_PRODUCT_CHU_720S 0x7001 +#define CMOTECH_PRODUCT_7002 0x7002 +#define CMOTECH_PRODUCT_CHU_629K 0x7003 +#define CMOTECH_PRODUCT_7004 0x7004 +#define CMOTECH_PRODUCT_7005 0x7005 +#define CMOTECH_PRODUCT_CGU_629 0x7006 +#define CMOTECH_PRODUCT_CHU_629S 0x700a +#define CMOTECH_PRODUCT_CHU_720I 0x7211 +#define CMOTECH_PRODUCT_7212 0x7212 +#define CMOTECH_PRODUCT_7213 0x7213 +#define CMOTECH_PRODUCT_7251 0x7251 +#define CMOTECH_PRODUCT_7252 0x7252 +#define CMOTECH_PRODUCT_7253 0x7253 + +#define TELIT_VENDOR_ID 0x1bc7 +#define TELIT_PRODUCT_UC864E 0x1003 +#define TELIT_PRODUCT_UC864G 0x1004 +#define TELIT_PRODUCT_CC864_DUAL 0x1005 +#define TELIT_PRODUCT_CC864_SINGLE 0x1006 +#define TELIT_PRODUCT_DE910_DUAL 0x1010 +#define TELIT_PRODUCT_UE910_V2 0x1012 +#define TELIT_PRODUCT_LE922_USBCFG1 0x1040 +#define TELIT_PRODUCT_LE922_USBCFG2 0x1041 +#define TELIT_PRODUCT_LE922_USBCFG0 0x1042 +#define TELIT_PRODUCT_LE922_USBCFG3 0x1043 +#define TELIT_PRODUCT_LE922_USBCFG5 0x1045 +#define TELIT_PRODUCT_ME910 0x1100 +#define TELIT_PRODUCT_ME910_DUAL_MODEM 0x1101 +#define TELIT_PRODUCT_LE920 0x1200 +#define TELIT_PRODUCT_LE910 0x1201 +#define TELIT_PRODUCT_LE910_USBCFG4 0x1206 +#define TELIT_PRODUCT_LE920A4_1207 0x1207 +#define TELIT_PRODUCT_LE920A4_1208 0x1208 +#define TELIT_PRODUCT_LE920A4_1211 0x1211 +#define TELIT_PRODUCT_LE920A4_1212 0x1212 +#define TELIT_PRODUCT_LE920A4_1213 0x1213 +#define TELIT_PRODUCT_LE920A4_1214 0x1214 + +/* ZTE PRODUCTS */ +#define ZTE_VENDOR_ID 0x19d2 +#define ZTE_PRODUCT_MF622 0x0001 +#define ZTE_PRODUCT_MF628 0x0015 +#define ZTE_PRODUCT_MF626 0x0031 +#define ZTE_PRODUCT_ZM8620_X 0x0396 +#define ZTE_PRODUCT_ME3620_MBIM 0x0426 +#define ZTE_PRODUCT_ME3620_X 0x1432 +#define ZTE_PRODUCT_ME3620_L 0x1433 +#define ZTE_PRODUCT_AC2726 0xfff1 +#define ZTE_PRODUCT_MG880 0xfffd +#define ZTE_PRODUCT_CDMA_TECH 0xfffe +#define ZTE_PRODUCT_AC8710T 0xffff +#define ZTE_PRODUCT_MC2718 0xffe8 +#define ZTE_PRODUCT_AD3812 0xffeb +#define ZTE_PRODUCT_MC2716 0xffed + +#define BENQ_VENDOR_ID 0x04a5 +#define BENQ_PRODUCT_H10 0x4068 + +#define DLINK_VENDOR_ID 0x1186 +#define DLINK_PRODUCT_DWM_652 0x3e04 +#define DLINK_PRODUCT_DWM_652_U5 0xce16 +#define DLINK_PRODUCT_DWM_652_U5A 0xce1e + +#define QISDA_VENDOR_ID 0x1da5 +#define QISDA_PRODUCT_H21_4512 0x4512 +#define QISDA_PRODUCT_H21_4523 0x4523 +#define QISDA_PRODUCT_H20_4515 0x4515 +#define QISDA_PRODUCT_H20_4518 0x4518 +#define QISDA_PRODUCT_H20_4519 0x4519 + +/* TLAYTECH PRODUCTS */ +#define TLAYTECH_VENDOR_ID 0x20B9 +#define TLAYTECH_PRODUCT_TEU800 0x1682 + +/* TOSHIBA PRODUCTS */ +#define TOSHIBA_VENDOR_ID 0x0930 +#define TOSHIBA_PRODUCT_HSDPA_MINICARD 0x1302 +#define TOSHIBA_PRODUCT_G450 0x0d45 + +#define ALINK_VENDOR_ID 0x1e0e +#define SIMCOM_PRODUCT_SIM7100E 0x9001 /* Yes, ALINK_VENDOR_ID */ +#define ALINK_PRODUCT_PH300 0x9100 +#define ALINK_PRODUCT_3GU 0x9200 + +/* ALCATEL PRODUCTS */ +#define ALCATEL_VENDOR_ID 0x1bbb +#define ALCATEL_PRODUCT_X060S_X200 0x0000 +#define ALCATEL_PRODUCT_X220_X500D 0x0017 +#define ALCATEL_PRODUCT_L100V 0x011e +#define ALCATEL_PRODUCT_L800MA 0x0203 + +#define PIRELLI_VENDOR_ID 0x1266 +#define PIRELLI_PRODUCT_C100_1 0x1002 +#define PIRELLI_PRODUCT_C100_2 0x1003 +#define PIRELLI_PRODUCT_1004 0x1004 +#define PIRELLI_PRODUCT_1005 0x1005 +#define PIRELLI_PRODUCT_1006 0x1006 +#define PIRELLI_PRODUCT_1007 0x1007 +#define PIRELLI_PRODUCT_1008 0x1008 +#define PIRELLI_PRODUCT_1009 0x1009 +#define PIRELLI_PRODUCT_100A 0x100a +#define PIRELLI_PRODUCT_100B 0x100b +#define PIRELLI_PRODUCT_100C 0x100c +#define PIRELLI_PRODUCT_100D 0x100d +#define PIRELLI_PRODUCT_100E 0x100e +#define PIRELLI_PRODUCT_100F 0x100f +#define PIRELLI_PRODUCT_1011 0x1011 +#define PIRELLI_PRODUCT_1012 0x1012 + +/* Airplus products */ +#define AIRPLUS_VENDOR_ID 0x1011 +#define AIRPLUS_PRODUCT_MCD650 0x3198 + +/* Longcheer/Longsung vendor ID; makes whitelabel devices that + * many other vendors like 4G Systems, Alcatel, ChinaBird, + * Mobidata, etc sell under their own brand names. + */ +#define LONGCHEER_VENDOR_ID 0x1c9e + +/* 4G Systems products */ +/* This is the 4G XS Stick W14 a.k.a. Mobilcom Debitel Surf-Stick * + * It seems to contain a Qualcomm QSC6240/6290 chipset */ +#define FOUR_G_SYSTEMS_PRODUCT_W14 0x9603 +#define FOUR_G_SYSTEMS_PRODUCT_W100 0x9b01 + +/* Fujisoft products */ +#define FUJISOFT_PRODUCT_FS040U 0x9b02 + +/* iBall 3.5G connect wireless modem */ +#define IBALL_3_5G_CONNECT 0x9605 + +/* Zoom */ +#define ZOOM_PRODUCT_4597 0x9607 + +/* SpeedUp SU9800 usb 3g modem */ +#define SPEEDUP_PRODUCT_SU9800 0x9800 + +/* Haier products */ +#define HAIER_VENDOR_ID 0x201e +#define HAIER_PRODUCT_CE81B 0x10f8 +#define HAIER_PRODUCT_CE100 0x2009 + +/* Gemalto's Cinterion products (formerly Siemens) */ +#define SIEMENS_VENDOR_ID 0x0681 +#define CINTERION_VENDOR_ID 0x1e2d +#define CINTERION_PRODUCT_HC25_MDMNET 0x0040 +#define CINTERION_PRODUCT_HC25_MDM 0x0047 +#define CINTERION_PRODUCT_HC28_MDMNET 0x004A /* same for HC28J */ +#define CINTERION_PRODUCT_HC28_MDM 0x004C +#define CINTERION_PRODUCT_EU3_E 0x0051 +#define CINTERION_PRODUCT_EU3_P 0x0052 +#define CINTERION_PRODUCT_PH8 0x0053 +#define CINTERION_PRODUCT_AHXX 0x0055 +#define CINTERION_PRODUCT_PLXX 0x0060 +#define CINTERION_PRODUCT_PH8_2RMNET 0x0082 +#define CINTERION_PRODUCT_PH8_AUDIO 0x0083 +#define CINTERION_PRODUCT_AHXX_2RMNET 0x0084 +#define CINTERION_PRODUCT_AHXX_AUDIO 0x0085 + +/* Olivetti products */ +#define OLIVETTI_VENDOR_ID 0x0b3c +#define OLIVETTI_PRODUCT_OLICARD100 0xc000 +#define OLIVETTI_PRODUCT_OLICARD120 0xc001 +#define OLIVETTI_PRODUCT_OLICARD140 0xc002 +#define OLIVETTI_PRODUCT_OLICARD145 0xc003 +#define OLIVETTI_PRODUCT_OLICARD155 0xc004 +#define OLIVETTI_PRODUCT_OLICARD200 0xc005 +#define OLIVETTI_PRODUCT_OLICARD160 0xc00a +#define OLIVETTI_PRODUCT_OLICARD500 0xc00b + +/* Celot products */ +#define CELOT_VENDOR_ID 0x211f +#define CELOT_PRODUCT_CT680M 0x6801 + +/* Samsung products */ +#define SAMSUNG_VENDOR_ID 0x04e8 +#define SAMSUNG_PRODUCT_GT_B3730 0x6889 + +/* YUGA products www.yuga-info.com gavin.kx@qq.com */ +#define YUGA_VENDOR_ID 0x257A +#define YUGA_PRODUCT_CEM600 0x1601 +#define YUGA_PRODUCT_CEM610 0x1602 +#define YUGA_PRODUCT_CEM500 0x1603 +#define YUGA_PRODUCT_CEM510 0x1604 +#define YUGA_PRODUCT_CEM800 0x1605 +#define YUGA_PRODUCT_CEM900 0x1606 + +#define YUGA_PRODUCT_CEU818 0x1607 +#define YUGA_PRODUCT_CEU816 0x1608 +#define YUGA_PRODUCT_CEU828 0x1609 +#define YUGA_PRODUCT_CEU826 0x160A +#define YUGA_PRODUCT_CEU518 0x160B +#define YUGA_PRODUCT_CEU516 0x160C +#define YUGA_PRODUCT_CEU528 0x160D +#define YUGA_PRODUCT_CEU526 0x160F +#define YUGA_PRODUCT_CEU881 0x161F +#define YUGA_PRODUCT_CEU882 0x162F + +#define YUGA_PRODUCT_CWM600 0x2601 +#define YUGA_PRODUCT_CWM610 0x2602 +#define YUGA_PRODUCT_CWM500 0x2603 +#define YUGA_PRODUCT_CWM510 0x2604 +#define YUGA_PRODUCT_CWM800 0x2605 +#define YUGA_PRODUCT_CWM900 0x2606 + +#define YUGA_PRODUCT_CWU718 0x2607 +#define YUGA_PRODUCT_CWU716 0x2608 +#define YUGA_PRODUCT_CWU728 0x2609 +#define YUGA_PRODUCT_CWU726 0x260A +#define YUGA_PRODUCT_CWU518 0x260B +#define YUGA_PRODUCT_CWU516 0x260C +#define YUGA_PRODUCT_CWU528 0x260D +#define YUGA_PRODUCT_CWU581 0x260E +#define YUGA_PRODUCT_CWU526 0x260F +#define YUGA_PRODUCT_CWU582 0x261F +#define YUGA_PRODUCT_CWU583 0x262F + +#define YUGA_PRODUCT_CLM600 0x3601 +#define YUGA_PRODUCT_CLM610 0x3602 +#define YUGA_PRODUCT_CLM500 0x3603 +#define YUGA_PRODUCT_CLM510 0x3604 +#define YUGA_PRODUCT_CLM800 0x3605 +#define YUGA_PRODUCT_CLM900 0x3606 + +#define YUGA_PRODUCT_CLU718 0x3607 +#define YUGA_PRODUCT_CLU716 0x3608 +#define YUGA_PRODUCT_CLU728 0x3609 +#define YUGA_PRODUCT_CLU726 0x360A +#define YUGA_PRODUCT_CLU518 0x360B +#define YUGA_PRODUCT_CLU516 0x360C +#define YUGA_PRODUCT_CLU528 0x360D +#define YUGA_PRODUCT_CLU526 0x360F + +/* Viettel products */ +#define VIETTEL_VENDOR_ID 0x2262 +#define VIETTEL_PRODUCT_VT1000 0x0002 + +/* ZD Incorporated */ +#define ZD_VENDOR_ID 0x0685 +#define ZD_PRODUCT_7000 0x7000 + +/* LG products */ +#define LG_VENDOR_ID 0x1004 +#define LG_PRODUCT_L02C 0x618f + +/* MediaTek products */ +#define MEDIATEK_VENDOR_ID 0x0e8d +#define MEDIATEK_PRODUCT_DC_1COM 0x00a0 +#define MEDIATEK_PRODUCT_DC_4COM 0x00a5 +#define MEDIATEK_PRODUCT_DC_4COM2 0x00a7 +#define MEDIATEK_PRODUCT_DC_5COM 0x00a4 +#define MEDIATEK_PRODUCT_7208_1COM 0x7101 +#define MEDIATEK_PRODUCT_7208_2COM 0x7102 +#define MEDIATEK_PRODUCT_7103_2COM 0x7103 +#define MEDIATEK_PRODUCT_7106_2COM 0x7106 +#define MEDIATEK_PRODUCT_FP_1COM 0x0003 +#define MEDIATEK_PRODUCT_FP_2COM 0x0023 +#define MEDIATEK_PRODUCT_FPDC_1COM 0x0043 +#define MEDIATEK_PRODUCT_FPDC_2COM 0x0033 + +/* Cellient products */ +#define CELLIENT_VENDOR_ID 0x2692 +#define CELLIENT_PRODUCT_MEN200 0x9005 + +/* Hyundai Petatel Inc. products */ +#define PETATEL_VENDOR_ID 0x1ff4 +#define PETATEL_PRODUCT_NP10T_600A 0x600a +#define PETATEL_PRODUCT_NP10T_600E 0x600e + +/* TP-LINK Incorporated products */ +#define TPLINK_VENDOR_ID 0x2357 +#define TPLINK_PRODUCT_LTE 0x000D +#define TPLINK_PRODUCT_MA180 0x0201 + +/* Changhong products */ +#define CHANGHONG_VENDOR_ID 0x2077 +#define CHANGHONG_PRODUCT_CH690 0x7001 + +/* Inovia */ +#define INOVIA_VENDOR_ID 0x20a6 +#define INOVIA_SEW858 0x1105 + +/* VIA Telecom */ +#define VIATELECOM_VENDOR_ID 0x15eb +#define VIATELECOM_PRODUCT_CDS7 0x0001 + +/* WeTelecom products */ +#define WETELECOM_VENDOR_ID 0x22de +#define WETELECOM_PRODUCT_WMD200 0x6801 +#define WETELECOM_PRODUCT_6802 0x6802 +#define WETELECOM_PRODUCT_WMD300 0x6803 + + +/* Device flags */ + +/* Interface does not support modem-control requests */ +#define NCTRL(ifnum) ((BIT(ifnum) & 0xff) << 8) + +/* Interface is reserved */ +#define RSVD(ifnum) ((BIT(ifnum) & 0xff) << 0) + + +static const struct usb_device_id option_ids[] = { + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_COLT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_LIGHT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_QUAD) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_QUAD_LIGHT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_NDIS) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_NDIS_LIGHT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_NDIS_QUAD) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA_NDIS_QUAD_LIGHT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_COBRA) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_COBRA_BUS) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_VIPER) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_VIPER_BUS) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_GT_MAX_READY) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_FUJI_MODEM_LIGHT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_FUJI_MODEM_GT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_FUJI_MODEM_EX) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_KOI_MODEM) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_SCORPION_MODEM) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_ETNA_MODEM) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_ETNA_MODEM_LITE) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_ETNA_MODEM_GT) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_ETNA_MODEM_EX) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_ETNA_KOI_MODEM) }, + { USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_GTM380_MODEM) }, + { USB_DEVICE(QUANTA_VENDOR_ID, QUANTA_PRODUCT_Q101) }, + { USB_DEVICE(QUANTA_VENDOR_ID, QUANTA_PRODUCT_Q111) }, + { USB_DEVICE(QUANTA_VENDOR_ID, QUANTA_PRODUCT_GLX) }, + { USB_DEVICE(QUANTA_VENDOR_ID, QUANTA_PRODUCT_GKE) }, + { USB_DEVICE(QUANTA_VENDOR_ID, QUANTA_PRODUCT_GLE) }, + { USB_DEVICE(QUANTA_VENDOR_ID, 0xea42), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0x1c05, USB_CLASS_COMM, 0x02, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0x1c1f, USB_CLASS_COMM, 0x02, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0x1c23, USB_CLASS_COMM, 0x02, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E173, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E173S6, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_E1750, 0xff, 0xff, 0xff), + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0x1441, USB_CLASS_COMM, 0x02, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0x1442, USB_CLASS_COMM, 0x02, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4505, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) | RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K3765, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) | RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0x14ac, 0xff, 0xff, 0xff), /* Huawei E1820 */ + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, HUAWEI_PRODUCT_K4605, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) | RSVD(2) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0xff, 0xff) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x02) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x03) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x04) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x06) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x0A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x0B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x0D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x0E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x0F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x10) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x12) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x13) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x14) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x15) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x17) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x18) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x19) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x1A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x1B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x1C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x31) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x32) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x33) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x34) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x35) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x36) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x3A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x3B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x3D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x3E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x3F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x48) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x49) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x4A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x4B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x4C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x61) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x62) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x63) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x64) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x65) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x66) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x6A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x6B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x6D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x6E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x6F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x72) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x73) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x74) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x75) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x78) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x79) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x7A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x7B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x01, 0x7C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x02) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x03) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x04) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x06) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x0A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x0B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x0D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x0E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x0F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x10) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x12) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x13) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x14) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x15) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x17) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x18) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x19) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x1A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x1B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x1C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x31) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x32) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x33) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x34) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x35) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x36) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x3A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x3B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x3D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x3E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x3F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x48) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x49) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x4A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x4B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x4C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x61) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x62) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x63) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x64) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x65) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x66) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x6A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x6B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x6D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x6E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x6F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x72) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x73) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x74) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x75) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x78) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x79) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x7A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x7B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x02, 0x7C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x02) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x03) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x04) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x06) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x0A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x0B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x0D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x0E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x0F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x10) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x12) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x13) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x14) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x15) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x17) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x18) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x19) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x1A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x1B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x1C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x31) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x32) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x33) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x34) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x35) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x36) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x3A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x3B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x3D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x3E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x3F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x48) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x49) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x4A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x4B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x4C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x61) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x62) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x63) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x64) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x65) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x66) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x6A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x6B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x6D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x6E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x6F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x72) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x73) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x74) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x75) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x78) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x79) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x7A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x7B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x03, 0x7C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x02) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x03) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x04) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x06) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x0A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x0B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x0D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x0E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x0F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x10) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x12) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x13) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x14) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x15) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x17) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x18) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x19) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x1A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x1B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x1C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x31) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x32) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x33) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x34) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x35) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x36) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x3A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x3B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x3D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x3E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x3F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x48) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x49) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x4A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x4B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x4C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x61) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x62) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x63) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x64) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x65) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x66) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x6A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x6B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x6D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x6E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x6F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x72) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x73) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x74) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x75) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x78) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x79) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x7A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x7B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x04, 0x7C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x02) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x03) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x04) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x06) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x0A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x0B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x0D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x0E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x0F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x10) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x12) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x13) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x14) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x15) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x17) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x18) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x19) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x1A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x1B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x1C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x31) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x32) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x33) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x34) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x35) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x36) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x3A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x3B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x3D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x3E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x3F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x48) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x49) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x4A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x4B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x4C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x61) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x62) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x63) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x64) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x65) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x66) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x6A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x6B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x6D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x6E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x6F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x72) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x73) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x74) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x75) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x78) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x79) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x7A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x7B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x05, 0x7C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x02) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x03) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x04) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x06) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x0A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x0B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x0D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x0E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x0F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x10) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x12) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x13) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x14) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x15) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x17) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x18) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x19) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x1A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x1B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x1C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x31) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x32) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x33) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x34) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x35) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x36) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x3A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x3B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x3D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x3E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x3F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x48) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x49) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x4A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x4B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x4C) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x61) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x62) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x63) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x64) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x65) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x66) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x6A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x6B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x6D) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x6E) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x6F) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x72) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x73) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x74) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x75) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x78) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x79) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x7A) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x7B) }, + { USB_VENDOR_AND_INTERFACE_INFO(HUAWEI_VENDOR_ID, 0xff, 0x06, 0x7C) }, + + + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V640) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V620) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V740) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_V720) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_U730) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_U740) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_U870) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_XU870) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_X950D) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EV620) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_ES720) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_E725) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_ES620) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EU730) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EU740) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EU870D) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC950D) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC727) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_OVMC760) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC780) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EVDO_FULLSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_FULLSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EVDO_EMBEDDED_FULLSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_FULLSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EVDO_HIGHSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED3) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED4) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED5) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED6) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_HIGHSPEED7) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC996D) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MF3470) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC547) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_EVDO_EMBEDDED_HIGHSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_HSPA_EMBEDDED_HIGHSPEED) }, + { USB_DEVICE(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_G2) }, + /* Novatel Ovation MC551 a.k.a. Verizon USB551L */ + { USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_MC551, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_E362, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_E371, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(NOVATELWIRELESS_VENDOR_ID, NOVATELWIRELESS_PRODUCT_U620L, 0xff, 0x00, 0x00) }, + + { USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H01) }, + { USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H01A) }, + { USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_H02) }, + { USB_DEVICE(AMOI_VENDOR_ID, AMOI_PRODUCT_SKYPEPHONE_S2) }, + + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5700_MINICARD) }, /* Dell Wireless 5700 Mobile Broadband CDMA/EVDO Mini-Card == Novatel Expedite EV620 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5500_MINICARD) }, /* Dell Wireless 5500 Mobile Broadband HSDPA Mini-Card == Novatel Expedite EU740 HSDPA/3G */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5505_MINICARD) }, /* Dell Wireless 5505 Mobile Broadband HSDPA Mini-Card == Novatel Expedite EU740 HSDPA/3G */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5700_EXPRESSCARD) }, /* Dell Wireless 5700 Mobile Broadband CDMA/EVDO ExpressCard == Novatel Merlin XV620 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5510_EXPRESSCARD) }, /* Dell Wireless 5510 Mobile Broadband HSDPA ExpressCard == Novatel Merlin XU870 HSDPA/3G */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5700_MINICARD_SPRINT) }, /* Dell Wireless 5700 Mobile Broadband CDMA/EVDO Mini-Card == Novatel Expedite E720 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5700_MINICARD_TELUS) }, /* Dell Wireless 5700 Mobile Broadband CDMA/EVDO Mini-Card == Novatel Expedite ET620 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5720_MINICARD_VZW) }, /* Dell Wireless 5720 == Novatel EV620 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5720_MINICARD_SPRINT) }, /* Dell Wireless 5720 == Novatel EV620 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5720_MINICARD_TELUS) }, /* Dell Wireless 5720 == Novatel EV620 CDMA/EV-DO */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5520_MINICARD_CINGULAR) }, /* Dell Wireless HSDPA 5520 == Novatel Expedite EU860D */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5520_MINICARD_GENERIC_L) }, /* Dell Wireless HSDPA 5520 */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5520_MINICARD_GENERIC_I) }, /* Dell Wireless 5520 Voda I Mobile Broadband (3G HSDPA) Minicard */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5730_MINICARD_SPRINT) }, /* Dell Wireless 5730 Mobile Broadband EVDO/HSPA Mini-Card */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5730_MINICARD_TELUS) }, /* Dell Wireless 5730 Mobile Broadband EVDO/HSPA Mini-Card */ + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5730_MINICARD_VZW) }, /* Dell Wireless 5730 Mobile Broadband EVDO/HSPA Mini-Card */ + { USB_DEVICE_AND_INTERFACE_INFO(DELL_VENDOR_ID, DELL_PRODUCT_5800_MINICARD_VZW, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(DELL_VENDOR_ID, DELL_PRODUCT_5800_V2_MINICARD_VZW, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(DELL_VENDOR_ID, DELL_PRODUCT_5804_MINICARD_ATT, 0xff, 0xff, 0xff) }, + { USB_DEVICE(DELL_VENDOR_ID, DELL_PRODUCT_5821E), + .driver_info = RSVD(0) | RSVD(1) | RSVD(6) }, + { USB_DEVICE(ANYDATA_VENDOR_ID, ANYDATA_PRODUCT_ADU_E100A) }, /* ADU-E100, ADU-310 */ + { USB_DEVICE(ANYDATA_VENDOR_ID, ANYDATA_PRODUCT_ADU_500A) }, + { USB_DEVICE(ANYDATA_VENDOR_ID, ANYDATA_PRODUCT_ADU_620UW) }, + { USB_DEVICE(AXESSTEL_VENDOR_ID, AXESSTEL_PRODUCT_MV110H) }, + { USB_DEVICE(YISO_VENDOR_ID, YISO_PRODUCT_U893) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_C100_1, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_C100_2, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1004, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1005, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1006, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1007, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1008, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1009, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_100A, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_100B, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_100C, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_100D, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_100E, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_100F, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1010, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1011, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1012, 0xff) }, + { USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC650) }, + { USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC680) }, + { USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6000)}, /* ZTE AC8700 */ + { USB_DEVICE_AND_INTERFACE_INFO(QUALCOMM_VENDOR_ID, 0x6001, 0xff, 0xff, 0xff), /* 4G LTE usb-modem U901 */ + .driver_info = RSVD(3) }, + { USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6613)}, /* Onda H600/ZTE MF330 */ + { USB_DEVICE(QUALCOMM_VENDOR_ID, 0x0023)}, /* ONYX 3G device */ + { USB_DEVICE(QUALCOMM_VENDOR_ID, 0x9000)}, /* SIMCom SIM5218 */ + /* Quectel products using Qualcomm vendor ID */ + { USB_DEVICE(QUALCOMM_VENDOR_ID, QUECTEL_PRODUCT_UC15)}, + { USB_DEVICE(QUALCOMM_VENDOR_ID, QUECTEL_PRODUCT_UC20), + .driver_info = RSVD(4) }, + /* Yuga products use Qualcomm vendor ID */ + { USB_DEVICE(QUALCOMM_VENDOR_ID, YUGA_PRODUCT_CLM920_NC5), + .driver_info = RSVD(1) | RSVD(4) }, + /* u-blox products using Qualcomm vendor ID */ + { USB_DEVICE(QUALCOMM_VENDOR_ID, UBLOX_PRODUCT_R410M), + .driver_info = RSVD(1) | RSVD(3) }, + /* Quectel products using Quectel vendor ID */ + { USB_DEVICE(QUECTEL_VENDOR_ID, QUECTEL_PRODUCT_EC21), + .driver_info = RSVD(4) }, + { USB_DEVICE(QUECTEL_VENDOR_ID, QUECTEL_PRODUCT_EC25), + .driver_info = RSVD(4) }, + { USB_DEVICE(QUECTEL_VENDOR_ID, QUECTEL_PRODUCT_BG96), + .driver_info = RSVD(4) }, + { USB_DEVICE(QUECTEL_VENDOR_ID, QUECTEL_PRODUCT_EP06), + .driver_info = RSVD(4) | RSVD(5) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6001) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CMU_300) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6003), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6004) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6005) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CGU_628A) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHE_628S), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CMU_301), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_628), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_628S) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CDU_680) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CDU_685A) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_720S), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7002), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_629K), + .driver_info = RSVD(4) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7004), + .driver_info = RSVD(3) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7005) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CGU_629), + .driver_info = RSVD(5) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_629S), + .driver_info = RSVD(4) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CHU_720I), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7212), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7213), + .driver_info = RSVD(0) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7251), + .driver_info = RSVD(1) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7252), + .driver_info = RSVD(1) }, + { USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_7253), + .driver_info = RSVD(1) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UC864E) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UC864G) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_CC864_DUAL) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_CC864_SINGLE) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_DE910_DUAL) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UE910_V2) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG0), + .driver_info = RSVD(0) | RSVD(1) | NCTRL(2) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG1), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG2), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG3), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) | RSVD(3) }, + { USB_DEVICE_INTERFACE_CLASS(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG5, 0xff), + .driver_info = RSVD(0) | RSVD(1) | NCTRL(2) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_ME910), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_ME910_DUAL_MODEM), + .driver_info = NCTRL(0) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE910), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE910_USBCFG4), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(5) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920A4_1207) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920A4_1208), + .driver_info = NCTRL(0) | RSVD(1) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920A4_1211), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) | RSVD(3) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920A4_1212), + .driver_info = NCTRL(0) | RSVD(1) }, + { USB_DEVICE_INTERFACE_CLASS(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920A4_1213, 0xff) }, + { USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920A4_1214), + .driver_info = NCTRL(0) | RSVD(1) | RSVD(2) | RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF622, 0xff, 0xff, 0xff) }, /* ZTE WCDMA products */ + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0002, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0003, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0004, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0005, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0006, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0008, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0009, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000a, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000b, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000c, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000d, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000e, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x000f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0010, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0011, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0012, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0013, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF628, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0016, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0017, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0018, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0019, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0020, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0021, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0022, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0023, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0024, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0025, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0028, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0029, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0030, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MF626, 0xff, 0xff, 0xff), + .driver_info = NCTRL(0) | NCTRL(1) | RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0032, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0033, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0034, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0037, 0xff, 0xff, 0xff), + .driver_info = NCTRL(0) | NCTRL(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0038, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0039, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0040, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0042, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0043, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0044, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0048, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0049, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0050, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0051, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0052, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0054, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0055, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0056, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0057, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0058, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0061, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0062, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0063, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0064, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0065, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0066, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0067, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0069, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0076, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0077, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0078, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0079, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0082, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0083, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0086, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0087, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0088, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0089, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0090, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0091, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0092, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0093, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0094, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0095, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0096, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0097, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0104, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0105, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0106, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0108, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0113, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0117, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0118, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0121, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0122, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0123, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0124, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0125, 0xff, 0xff, 0xff), + .driver_info = RSVD(6) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0126, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0128, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0135, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0136, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0137, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0139, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0142, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0143, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0144, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0145, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0148, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0151, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0153, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0155, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0156, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0157, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0158, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0159, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0161, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0162, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0164, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0165, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0167, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0189, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0191, 0xff, 0xff, 0xff), /* ZTE EuFi890 */ + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0196, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0197, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0199, 0xff, 0xff, 0xff), /* ZTE MF820S */ + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0200, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0201, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0254, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0257, 0xff, 0xff, 0xff), /* ZTE MF821 */ + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0265, 0xff, 0xff, 0xff), /* ONDA MT8205 */ + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0284, 0xff, 0xff, 0xff), /* ZTE MF880 */ + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0317, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0326, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0330, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0395, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0412, 0xff, 0xff, 0xff), /* Telewell TW-LTE 4G */ + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0414, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0417, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1008, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1010, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1012, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1018, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1021, 0xff, 0xff, 0xff), + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1057, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1058, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1059, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1060, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1061, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1062, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1063, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1064, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1065, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1066, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1067, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1068, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1069, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1070, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1071, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1072, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1073, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1074, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1075, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1076, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1077, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1078, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1079, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1080, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1081, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1082, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1083, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1084, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1085, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1086, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1087, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1088, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1089, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1090, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1091, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1092, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1093, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1094, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1095, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1096, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1097, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1098, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1099, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1100, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1101, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1102, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1103, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1104, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1105, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1106, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1107, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1108, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1109, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1110, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1111, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1112, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1113, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1114, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1115, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1116, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1117, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1118, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1119, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1120, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1121, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1122, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1123, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1124, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1125, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1126, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1127, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1128, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1129, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1130, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1131, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1132, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1133, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1134, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1135, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1136, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1137, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1138, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1139, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1140, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1141, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1142, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1143, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1144, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1145, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1146, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1147, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1148, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1149, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1150, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1151, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1152, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1153, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1154, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1155, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1156, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1157, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1158, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1159, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1160, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1161, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1162, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1163, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1164, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1165, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1166, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1167, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1168, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1169, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1170, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1244, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1245, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1246, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1247, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1248, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1249, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1250, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1251, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1252, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1253, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1254, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1255, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) | RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1256, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1257, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1258, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1259, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1260, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1261, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1262, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1263, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1264, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1265, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1266, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1267, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1268, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1269, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1270, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1271, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1272, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1273, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1274, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1275, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1276, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1277, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1278, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1279, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1280, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1281, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1282, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1283, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1284, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1285, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1286, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1287, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1288, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1289, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1290, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1291, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1292, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1293, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1294, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1295, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1296, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1297, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1298, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1299, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1300, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1301, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1302, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1303, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1333, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1401, 0xff, 0xff, 0xff), + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1402, 0xff, 0xff, 0xff), + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1424, 0xff, 0xff, 0xff), + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1425, 0xff, 0xff, 0xff), + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1426, 0xff, 0xff, 0xff), /* ZTE MF91 */ + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1428, 0xff, 0xff, 0xff), /* Telewell TW-LTE 4G v2 */ + .driver_info = RSVD(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1533, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1534, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1535, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1545, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1546, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1547, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1565, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1566, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1567, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1589, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1590, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1591, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1592, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1594, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1596, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1598, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x1600, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x2002, 0xff, 0xff, 0xff), + .driver_info = NCTRL(0) | NCTRL(1) | NCTRL(2) | RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x2003, 0xff, 0xff, 0xff) }, + + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0014, 0xff, 0xff, 0xff) }, /* ZTE CDMA products */ + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0027, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0059, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0060, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0070, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0073, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0094, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0130, 0xff, 0xff, 0xff), + .driver_info = RSVD(1) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0133, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0141, 0xff, 0xff, 0xff), + .driver_info = RSVD(5) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0147, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0152, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0168, 0xff, 0xff, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0170, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0176, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0x0178, 0xff, 0xff, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff42, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff43, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff44, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff45, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff46, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff47, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff48, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff49, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff4a, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff4b, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff4c, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff4d, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff4e, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff4f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff50, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff51, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff52, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff53, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff54, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff55, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff56, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff57, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff58, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff59, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff5a, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff5b, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff5c, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff5d, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff5e, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff5f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff60, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff61, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff62, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff63, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff64, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff65, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff66, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff67, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff68, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff69, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff6a, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff6b, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff6c, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff6d, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff6e, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff6f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff70, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff71, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff72, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff73, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff74, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff75, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff76, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff77, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff78, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff79, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff7a, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff7b, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff7c, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff7d, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff7e, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff7f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff80, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff81, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff82, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff83, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff84, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff85, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff86, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff87, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff88, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff89, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff8a, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff8b, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff8c, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff8d, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff8e, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff8f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff90, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff91, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff92, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff93, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff94, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff9f, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa0, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa1, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa2, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa3, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa4, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa5, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa6, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa7, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa8, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffa9, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffaa, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffab, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffac, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffae, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffaf, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb0, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb1, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb2, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb3, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb4, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb5, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb6, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb7, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb8, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffb9, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffba, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffbb, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffbc, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffbd, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffbe, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffbf, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc0, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc1, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc2, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc3, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc4, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc5, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc6, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc7, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc8, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffc9, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffca, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffcb, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffcc, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffcd, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffce, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffcf, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffd0, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffd1, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffd2, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffd3, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffd4, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffd5, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffe9, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffec, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffee, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff6, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff7, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff8, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff9, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfffb, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfffc, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MG880, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_CDMA_TECH, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC2726, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC8710T, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MC2718, 0xff, 0xff, 0xff), + .driver_info = NCTRL(1) | NCTRL(2) | NCTRL(3) | NCTRL(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AD3812, 0xff, 0xff, 0xff), + .driver_info = NCTRL(0) | NCTRL(1) | NCTRL(2) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MC2716, 0xff, 0xff, 0xff), + .driver_info = NCTRL(1) | NCTRL(2) | NCTRL(3) }, + { USB_DEVICE(ZTE_VENDOR_ID, ZTE_PRODUCT_ME3620_L), + .driver_info = RSVD(3) | RSVD(4) | RSVD(5) }, + { USB_DEVICE(ZTE_VENDOR_ID, ZTE_PRODUCT_ME3620_MBIM), + .driver_info = RSVD(2) | RSVD(3) | RSVD(4) }, + { USB_DEVICE(ZTE_VENDOR_ID, ZTE_PRODUCT_ME3620_X), + .driver_info = RSVD(3) | RSVD(4) | RSVD(5) }, + { USB_DEVICE(ZTE_VENDOR_ID, ZTE_PRODUCT_ZM8620_X), + .driver_info = RSVD(3) | RSVD(4) | RSVD(5) }, + { USB_VENDOR_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff, 0x02, 0x01) }, + { USB_VENDOR_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff, 0x02, 0x05) }, + { USB_VENDOR_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff, 0x86, 0x10) }, + + { USB_DEVICE(BENQ_VENDOR_ID, BENQ_PRODUCT_H10) }, + { USB_DEVICE(DLINK_VENDOR_ID, DLINK_PRODUCT_DWM_652) }, + { USB_DEVICE(ALINK_VENDOR_ID, DLINK_PRODUCT_DWM_652_U5) }, /* Yes, ALINK_VENDOR_ID */ + { USB_DEVICE(ALINK_VENDOR_ID, DLINK_PRODUCT_DWM_652_U5A) }, + { USB_DEVICE(QISDA_VENDOR_ID, QISDA_PRODUCT_H21_4512) }, + { USB_DEVICE(QISDA_VENDOR_ID, QISDA_PRODUCT_H21_4523) }, + { USB_DEVICE(QISDA_VENDOR_ID, QISDA_PRODUCT_H20_4515) }, + { USB_DEVICE(QISDA_VENDOR_ID, QISDA_PRODUCT_H20_4518) }, + { USB_DEVICE(QISDA_VENDOR_ID, QISDA_PRODUCT_H20_4519) }, + { USB_DEVICE(TOSHIBA_VENDOR_ID, TOSHIBA_PRODUCT_G450) }, + { USB_DEVICE(TOSHIBA_VENDOR_ID, TOSHIBA_PRODUCT_HSDPA_MINICARD ) }, /* Toshiba 3G HSDPA == Novatel Expedite EU870D MiniCard */ + { USB_DEVICE(ALINK_VENDOR_ID, 0x9000) }, + { USB_DEVICE(ALINK_VENDOR_ID, ALINK_PRODUCT_PH300) }, + { USB_DEVICE_AND_INTERFACE_INFO(ALINK_VENDOR_ID, ALINK_PRODUCT_3GU, 0xff, 0xff, 0xff) }, + { USB_DEVICE(ALINK_VENDOR_ID, SIMCOM_PRODUCT_SIM7100E), + .driver_info = RSVD(5) | RSVD(6) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_X060S_X200), + .driver_info = NCTRL(0) | NCTRL(1) | RSVD(4) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_X220_X500D), + .driver_info = RSVD(6) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, 0x0052), + .driver_info = RSVD(6) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, 0x00b6), + .driver_info = RSVD(3) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, 0x00b7), + .driver_info = RSVD(5) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_L100V), + .driver_info = RSVD(4) }, + { USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_L800MA), + .driver_info = RSVD(2) }, + { USB_DEVICE(AIRPLUS_VENDOR_ID, AIRPLUS_PRODUCT_MCD650) }, + { USB_DEVICE(TLAYTECH_VENDOR_ID, TLAYTECH_PRODUCT_TEU800) }, + { USB_DEVICE(LONGCHEER_VENDOR_ID, FOUR_G_SYSTEMS_PRODUCT_W14), + .driver_info = NCTRL(0) | NCTRL(1) }, + { USB_DEVICE(LONGCHEER_VENDOR_ID, FOUR_G_SYSTEMS_PRODUCT_W100), + .driver_info = NCTRL(1) | NCTRL(2) | RSVD(3) }, + {USB_DEVICE(LONGCHEER_VENDOR_ID, FUJISOFT_PRODUCT_FS040U), + .driver_info = RSVD(3)}, + { USB_DEVICE_INTERFACE_CLASS(LONGCHEER_VENDOR_ID, SPEEDUP_PRODUCT_SU9800, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(LONGCHEER_VENDOR_ID, 0x9801, 0xff), + .driver_info = RSVD(3) }, + { USB_DEVICE_INTERFACE_CLASS(LONGCHEER_VENDOR_ID, 0x9803, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE(LONGCHEER_VENDOR_ID, ZOOM_PRODUCT_4597) }, + { USB_DEVICE(LONGCHEER_VENDOR_ID, IBALL_3_5G_CONNECT) }, + { USB_DEVICE(HAIER_VENDOR_ID, HAIER_PRODUCT_CE100) }, + { USB_DEVICE_AND_INTERFACE_INFO(HAIER_VENDOR_ID, HAIER_PRODUCT_CE81B, 0xff, 0xff, 0xff) }, + /* Pirelli */ + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_C100_1, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_C100_2, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1004, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1005, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1006, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1007, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1008, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1009, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_100A, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_100B, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_100C, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_100D, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_100E, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_100F, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1011, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(PIRELLI_VENDOR_ID, PIRELLI_PRODUCT_1012, 0xff) }, + /* Cinterion */ + { USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_EU3_E) }, + { USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_EU3_P) }, + { USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_PH8), + .driver_info = RSVD(4) }, + { USB_DEVICE_INTERFACE_CLASS(CINTERION_VENDOR_ID, CINTERION_PRODUCT_AHXX, 0xff) }, + { USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_PLXX), + .driver_info = RSVD(4) }, + { USB_DEVICE_INTERFACE_CLASS(CINTERION_VENDOR_ID, CINTERION_PRODUCT_PH8_2RMNET, 0xff), + .driver_info = RSVD(4) | RSVD(5) }, + { USB_DEVICE_INTERFACE_CLASS(CINTERION_VENDOR_ID, CINTERION_PRODUCT_PH8_AUDIO, 0xff), + .driver_info = RSVD(4) }, + { USB_DEVICE_INTERFACE_CLASS(CINTERION_VENDOR_ID, CINTERION_PRODUCT_AHXX_2RMNET, 0xff) }, + { USB_DEVICE_INTERFACE_CLASS(CINTERION_VENDOR_ID, CINTERION_PRODUCT_AHXX_AUDIO, 0xff) }, + { USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_HC28_MDM) }, + { USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_HC28_MDMNET) }, + { USB_DEVICE(SIEMENS_VENDOR_ID, CINTERION_PRODUCT_HC25_MDM) }, + { USB_DEVICE(SIEMENS_VENDOR_ID, CINTERION_PRODUCT_HC25_MDMNET) }, + { USB_DEVICE(SIEMENS_VENDOR_ID, CINTERION_PRODUCT_HC28_MDM) }, /* HC28 enumerates with Siemens or Cinterion VID depending on FW revision */ + { USB_DEVICE(SIEMENS_VENDOR_ID, CINTERION_PRODUCT_HC28_MDMNET) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD100), + .driver_info = RSVD(4) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD120), + .driver_info = RSVD(4) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD140), + .driver_info = RSVD(4) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD145) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD155), + .driver_info = RSVD(6) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD200), + .driver_info = RSVD(6) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD160), + .driver_info = RSVD(6) }, + { USB_DEVICE(OLIVETTI_VENDOR_ID, OLIVETTI_PRODUCT_OLICARD500), + .driver_info = RSVD(4) }, + { USB_DEVICE(CELOT_VENDOR_ID, CELOT_PRODUCT_CT680M) }, /* CT-650 CDMA 450 1xEVDO modem */ + { USB_DEVICE_AND_INTERFACE_INFO(SAMSUNG_VENDOR_ID, SAMSUNG_PRODUCT_GT_B3730, USB_CLASS_CDC_DATA, 0x00, 0x00) }, /* Samsung GT-B3730 LTE USB modem.*/ + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM600) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM610) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM500) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM510) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM800) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEM900) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU818) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU816) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU828) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU826) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU518) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU516) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU528) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU526) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWM600) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWM610) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWM500) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWM510) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWM800) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWM900) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU718) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU716) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU728) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU726) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU518) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU516) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU528) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU526) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLM600) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLM610) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLM500) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLM510) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLM800) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLM900) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU718) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU716) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU728) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU726) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU518) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU516) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU528) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CLU526) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU881) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CEU882) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU581) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU582) }, + { USB_DEVICE(YUGA_VENDOR_ID, YUGA_PRODUCT_CWU583) }, + { USB_DEVICE_AND_INTERFACE_INFO(VIETTEL_VENDOR_ID, VIETTEL_PRODUCT_VT1000, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(ZD_VENDOR_ID, ZD_PRODUCT_7000, 0xff, 0xff, 0xff) }, + { USB_DEVICE(LG_VENDOR_ID, LG_PRODUCT_L02C) }, /* docomo L-02C modem */ + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, 0x00a1, 0xff, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, 0x00a1, 0xff, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, 0x00a2, 0xff, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, 0x00a2, 0xff, 0x02, 0x01) }, /* MediaTek MT6276M modem & app port */ + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_1COM, 0x0a, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_5COM, 0xff, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_5COM, 0xff, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_4COM, 0xff, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_4COM, 0xff, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_7208_1COM, 0x02, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_7208_2COM, 0x02, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_FP_1COM, 0x0a, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_FP_2COM, 0x0a, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_FPDC_1COM, 0x0a, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_FPDC_2COM, 0x0a, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_7103_2COM, 0xff, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_7106_2COM, 0x02, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_4COM2, 0xff, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(MEDIATEK_VENDOR_ID, MEDIATEK_PRODUCT_DC_4COM2, 0xff, 0x00, 0x00) }, + { USB_DEVICE(CELLIENT_VENDOR_ID, CELLIENT_PRODUCT_MEN200) }, + { USB_DEVICE(PETATEL_VENDOR_ID, PETATEL_PRODUCT_NP10T_600A) }, + { USB_DEVICE(PETATEL_VENDOR_ID, PETATEL_PRODUCT_NP10T_600E) }, + { USB_DEVICE_AND_INTERFACE_INFO(TPLINK_VENDOR_ID, TPLINK_PRODUCT_LTE, 0xff, 0x00, 0x00) }, /* TP-Link LTE Module */ + { USB_DEVICE(TPLINK_VENDOR_ID, TPLINK_PRODUCT_MA180), + .driver_info = RSVD(4) }, + { USB_DEVICE(TPLINK_VENDOR_ID, 0x9000), /* TP-Link MA260 */ + .driver_info = RSVD(4) }, + { USB_DEVICE(CHANGHONG_VENDOR_ID, CHANGHONG_PRODUCT_CH690) }, + { USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d01, 0xff, 0x02, 0x01) }, /* D-Link DWM-156 (variant) */ + { USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d01, 0xff, 0x00, 0x00) }, /* D-Link DWM-156 (variant) */ + { USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d02, 0xff, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d02, 0xff, 0x00, 0x00) }, + { USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d03, 0xff, 0x02, 0x01) }, + { USB_DEVICE_AND_INTERFACE_INFO(0x2001, 0x7d03, 0xff, 0x00, 0x00) }, + { USB_DEVICE_INTERFACE_CLASS(0x2001, 0x7d04, 0xff) }, /* D-Link DWM-158 */ + { USB_DEVICE_INTERFACE_CLASS(0x2001, 0x7d0e, 0xff) }, /* D-Link DWM-157 C1 */ + { USB_DEVICE_INTERFACE_CLASS(0x2001, 0x7e19, 0xff), /* D-Link DWM-221 B1 */ + .driver_info = RSVD(4) }, + { USB_DEVICE_INTERFACE_CLASS(0x2001, 0x7e35, 0xff), /* D-Link DWM-222 */ + .driver_info = RSVD(4) }, + { USB_DEVICE_AND_INTERFACE_INFO(0x07d1, 0x3e01, 0xff, 0xff, 0xff) }, /* D-Link DWM-152/C1 */ + { USB_DEVICE_AND_INTERFACE_INFO(0x07d1, 0x3e02, 0xff, 0xff, 0xff) }, /* D-Link DWM-156/C1 */ + { USB_DEVICE_AND_INTERFACE_INFO(0x07d1, 0x7e11, 0xff, 0xff, 0xff) }, /* D-Link DWM-156/A3 */ + { USB_DEVICE_INTERFACE_CLASS(0x2020, 0x4000, 0xff) }, /* OLICARD300 - MT6225 */ + { USB_DEVICE(INOVIA_VENDOR_ID, INOVIA_SEW858) }, + { USB_DEVICE(VIATELECOM_VENDOR_ID, VIATELECOM_PRODUCT_CDS7) }, + { USB_DEVICE_AND_INTERFACE_INFO(WETELECOM_VENDOR_ID, WETELECOM_PRODUCT_WMD200, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(WETELECOM_VENDOR_ID, WETELECOM_PRODUCT_6802, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(WETELECOM_VENDOR_ID, WETELECOM_PRODUCT_WMD300, 0xff, 0xff, 0xff) }, + { USB_DEVICE_AND_INTERFACE_INFO(0x03f0, 0x421d, 0xff, 0xff, 0xff) }, /* HP lt2523 (Novatel E371) */ + #if 1 + { USB_DEVICE(0x05C6, 0x9090) }, /* Quectel UC15 */ + { USB_DEVICE(0x05C6, 0x9003) }, /* Quectel UC20 */ + { USB_DEVICE(0x05C6, 0x9215) }, /* Quectel EC20 */ + { USB_DEVICE(0x2C7C, 0x0125) }, /* Quectel EC25/EC20 R2.0 */ + { USB_DEVICE(0x2C7C, 0x0121) }, /* Quectel EC21 */ + #endif + { } /* Terminating entry */ +}; +MODULE_DEVICE_TABLE(usb, option_ids); + +/* The card has three separate interfaces, which the serial driver + * recognizes separately, thus num_port=1. + */ + +static struct usb_serial_driver option_1port_device = { + .driver = { + .owner = THIS_MODULE, + .name = "option1", + }, + .description = "GSM modem (1-port)", + .id_table = option_ids, + .num_ports = 1, + .probe = option_probe, + .open = usb_wwan_open, + .close = usb_wwan_close, + .dtr_rts = usb_wwan_dtr_rts, + .write = usb_wwan_write, + .write_room = usb_wwan_write_room, + .chars_in_buffer = usb_wwan_chars_in_buffer, + .tiocmget = usb_wwan_tiocmget, + .tiocmset = usb_wwan_tiocmset, + .ioctl = usb_wwan_ioctl, + .attach = option_attach, + .release = option_release, + .port_probe = usb_wwan_port_probe, + .port_remove = usb_wwan_port_remove, + .read_int_callback = option_instat_callback, +#ifdef CONFIG_PM + .suspend = usb_wwan_suspend, + .resume = usb_wwan_resume, +#if 1 //Added by Quectel + .reset_resume = usb_wwan_resume, +#endif +#endif +}; + +static struct usb_serial_driver * const serial_drivers[] = { + &option_1port_device, NULL +}; + +module_usb_serial_driver(serial_drivers, option_ids); + +static int option_probe(struct usb_serial *serial, + const struct usb_device_id *id) +{ + struct usb_interface_descriptor *iface_desc = + &serial->interface->cur_altsetting->desc; + struct usb_device_descriptor *dev_desc = &serial->dev->descriptor; + unsigned long device_flags = id->driver_info; + + /* Never bind to the CD-Rom emulation interface */ + if (iface_desc->bInterfaceClass == 0x08) + return -ENODEV; + + /* + * Don't bind reserved interfaces (like network ones) which often have + * the same class/subclass/protocol as the serial interfaces. Look at + * the Windows driver .INF files for reserved interface numbers. + */ + if (device_flags & RSVD(iface_desc->bInterfaceNumber)) + return -ENODEV; + /* + * Don't bind network interface on Samsung GT-B3730, it is handled by + * a separate module. + */ + if (dev_desc->idVendor == cpu_to_le16(SAMSUNG_VENDOR_ID) && + dev_desc->idProduct == cpu_to_le16(SAMSUNG_PRODUCT_GT_B3730) && + iface_desc->bInterfaceClass != USB_CLASS_CDC_DATA) + return -ENODEV; + + #if 1 //Added by Quectel + //Quectel EC21&EC25&EC20 R2.0's interface 4 can be used as USB Network device + if (serial->dev->descriptor.idVendor == cpu_to_le16(0x2C7C) && + serial->interface->cur_altsetting->desc.bInterfaceNumber >= 4) + return -ENODEV; + #endif + /* Store the device flags so we can use them during attach. */ + usb_set_serial_data(serial, (void *)device_flags); + + return 0; +} + +static int option_attach(struct usb_serial *serial) +{ + struct usb_interface_descriptor *iface_desc; + struct usb_wwan_intf_private *data; + unsigned long device_flags; + + data = kzalloc(sizeof(struct usb_wwan_intf_private), GFP_KERNEL); + if (!data) + return -ENOMEM; + + /* Retrieve device flags stored at probe. */ + device_flags = (unsigned long)usb_get_serial_data(serial); + + iface_desc = &serial->interface->cur_altsetting->desc; + + if (!(device_flags & NCTRL(iface_desc->bInterfaceNumber))) + data->use_send_setup = 1; + + spin_lock_init(&data->susp_lock); + + usb_set_serial_data(serial, data); + + return 0; +} + +static void option_release(struct usb_serial *serial) +{ + struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial); + + kfree(intfdata); +} + +static void option_instat_callback(struct urb *urb) +{ + int err; + int status = urb->status; + struct usb_serial_port *port = urb->context; + struct device *dev = &port->dev; + struct usb_wwan_port_private *portdata = + usb_get_serial_port_data(port); + + dev_dbg(dev, "%s: urb %p port %p has data %p\n", __func__, urb, port, portdata); + + if (status == 0) { + struct usb_ctrlrequest *req_pkt = + (struct usb_ctrlrequest *)urb->transfer_buffer; + + if (!req_pkt) { + dev_dbg(dev, "%s: NULL req_pkt\n", __func__); + return; + } + if ((req_pkt->bRequestType == 0xA1) && + (req_pkt->bRequest == 0x20)) { + int old_dcd_state; + unsigned char signals = *((unsigned char *) + urb->transfer_buffer + + sizeof(struct usb_ctrlrequest)); + + dev_dbg(dev, "%s: signal x%x\n", __func__, signals); + + old_dcd_state = portdata->dcd_state; + portdata->cts_state = 1; + portdata->dcd_state = ((signals & 0x01) ? 1 : 0); + portdata->dsr_state = ((signals & 0x02) ? 1 : 0); + portdata->ri_state = ((signals & 0x08) ? 1 : 0); + + if (old_dcd_state && !portdata->dcd_state) + tty_port_tty_hangup(&port->port, true); + } else { + dev_dbg(dev, "%s: type %x req %x\n", __func__, + req_pkt->bRequestType, req_pkt->bRequest); + } + } else if (status == -ENOENT || status == -ESHUTDOWN) { + dev_dbg(dev, "%s: urb stopped: %d\n", __func__, status); + } else + dev_dbg(dev, "%s: error %d\n", __func__, status); + + /* Resubmit urb so we continue receiving IRQ data */ + if (status != -ESHUTDOWN && status != -ENOENT) { + usb_mark_last_busy(port->serial->dev); + err = usb_submit_urb(urb, GFP_ATOMIC); + if (err) + dev_dbg(dev, "%s: resubmit intr urb failed. (%d)\n", + __func__, err); + } +} + +MODULE_AUTHOR(DRIVER_AUTHOR); +MODULE_DESCRIPTION(DRIVER_DESC); +MODULE_LICENSE("GPL"); diff --git a/drivers/usb/serial/usb_wwan.c b/drivers/usb/serial/usb_wwan.c new file mode 100644 index 0000000..f217f15 --- /dev/null +++ b/drivers/usb/serial/usb_wwan.c @@ -0,0 +1,742 @@ +/* + USB Driver layer for GSM modems + + Copyright (C) 2005 Matthias Urlichs + + This driver is free software; you can redistribute it and/or modify + it under the terms of Version 2 of the GNU General Public License as + published by the Free Software Foundation. + + Portions copied from the Keyspan driver by Hugh Blemings + + History: see the git log. + + Work sponsored by: Sigos GmbH, Germany + + This driver exists because the "normal" serial driver doesn't work too well + with GSM modems. Issues: + - data loss -- one single Receive URB is not nearly enough + - controlling the baud rate doesn't make sense +*/ + +#define DRIVER_AUTHOR "Matthias Urlichs " +#define DRIVER_DESC "USB Driver for GSM modems" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "usb-wwan.h" + +/* + * Generate DTR/RTS signals on the port using the SET_CONTROL_LINE_STATE request + * in CDC ACM. + */ +static int usb_wwan_send_setup(struct usb_serial_port *port) +{ + struct usb_serial *serial = port->serial; + struct usb_wwan_port_private *portdata; + int val = 0; + int ifnum; + int res; + + portdata = usb_get_serial_port_data(port); + + if (portdata->dtr_state) + val |= 0x01; + if (portdata->rts_state) + val |= 0x02; + + ifnum = serial->interface->cur_altsetting->desc.bInterfaceNumber; + + res = usb_autopm_get_interface(serial->interface); + if (res) + return res; + + res = usb_control_msg(serial->dev, usb_sndctrlpipe(serial->dev, 0), + 0x22, 0x21, val, ifnum, NULL, 0, + USB_CTRL_SET_TIMEOUT); + + usb_autopm_put_interface(port->serial->interface); + + return res; +} + +void usb_wwan_dtr_rts(struct usb_serial_port *port, int on) +{ + struct usb_wwan_port_private *portdata; + struct usb_wwan_intf_private *intfdata; + + intfdata = usb_get_serial_data(port->serial); + + if (!intfdata->use_send_setup) + return; + + portdata = usb_get_serial_port_data(port); + /* FIXME: locking */ + portdata->rts_state = on; + portdata->dtr_state = on; + + usb_wwan_send_setup(port); +} +EXPORT_SYMBOL(usb_wwan_dtr_rts); + +int usb_wwan_tiocmget(struct tty_struct *tty) +{ + struct usb_serial_port *port = tty->driver_data; + unsigned int value; + struct usb_wwan_port_private *portdata; + + portdata = usb_get_serial_port_data(port); + + value = ((portdata->rts_state) ? TIOCM_RTS : 0) | + ((portdata->dtr_state) ? TIOCM_DTR : 0) | + ((portdata->cts_state) ? TIOCM_CTS : 0) | + ((portdata->dsr_state) ? TIOCM_DSR : 0) | + ((portdata->dcd_state) ? TIOCM_CAR : 0) | + ((portdata->ri_state) ? TIOCM_RNG : 0); + + return value; +} +EXPORT_SYMBOL(usb_wwan_tiocmget); + +int usb_wwan_tiocmset(struct tty_struct *tty, + unsigned int set, unsigned int clear) +{ + struct usb_serial_port *port = tty->driver_data; + struct usb_wwan_port_private *portdata; + struct usb_wwan_intf_private *intfdata; + + portdata = usb_get_serial_port_data(port); + intfdata = usb_get_serial_data(port->serial); + + if (!intfdata->use_send_setup) + return -EINVAL; + + /* FIXME: what locks portdata fields ? */ + if (set & TIOCM_RTS) + portdata->rts_state = 1; + if (set & TIOCM_DTR) + portdata->dtr_state = 1; + + if (clear & TIOCM_RTS) + portdata->rts_state = 0; + if (clear & TIOCM_DTR) + portdata->dtr_state = 0; + return usb_wwan_send_setup(port); +} +EXPORT_SYMBOL(usb_wwan_tiocmset); + +static int get_serial_info(struct usb_serial_port *port, + struct serial_struct __user *retinfo) +{ + struct serial_struct tmp; + + if (!retinfo) + return -EFAULT; + + memset(&tmp, 0, sizeof(tmp)); + tmp.line = port->minor; + tmp.port = port->port_number; + tmp.baud_base = tty_get_baud_rate(port->port.tty); + tmp.close_delay = port->port.close_delay / 10; + tmp.closing_wait = port->port.closing_wait == ASYNC_CLOSING_WAIT_NONE ? + ASYNC_CLOSING_WAIT_NONE : + port->port.closing_wait / 10; + + if (copy_to_user(retinfo, &tmp, sizeof(*retinfo))) + return -EFAULT; + return 0; +} + +static int set_serial_info(struct usb_serial_port *port, + struct serial_struct __user *newinfo) +{ + struct serial_struct new_serial; + unsigned int closing_wait, close_delay; + int retval = 0; + + if (copy_from_user(&new_serial, newinfo, sizeof(new_serial))) + return -EFAULT; + + close_delay = new_serial.close_delay * 10; + closing_wait = new_serial.closing_wait == ASYNC_CLOSING_WAIT_NONE ? + ASYNC_CLOSING_WAIT_NONE : new_serial.closing_wait * 10; + + mutex_lock(&port->port.mutex); + + if (!capable(CAP_SYS_ADMIN)) { + if ((close_delay != port->port.close_delay) || + (closing_wait != port->port.closing_wait)) + retval = -EPERM; + else + retval = -EOPNOTSUPP; + } else { + port->port.close_delay = close_delay; + port->port.closing_wait = closing_wait; + } + + mutex_unlock(&port->port.mutex); + return retval; +} + +int usb_wwan_ioctl(struct tty_struct *tty, + unsigned int cmd, unsigned long arg) +{ + struct usb_serial_port *port = tty->driver_data; + + dev_dbg(&port->dev, "%s cmd 0x%04x\n", __func__, cmd); + + switch (cmd) { + case TIOCGSERIAL: + return get_serial_info(port, + (struct serial_struct __user *) arg); + case TIOCSSERIAL: + return set_serial_info(port, + (struct serial_struct __user *) arg); + default: + break; + } + + dev_dbg(&port->dev, "%s arg not supported\n", __func__); + + return -ENOIOCTLCMD; +} +EXPORT_SYMBOL(usb_wwan_ioctl); + +int usb_wwan_write(struct tty_struct *tty, struct usb_serial_port *port, + const unsigned char *buf, int count) +{ + struct usb_wwan_port_private *portdata; + struct usb_wwan_intf_private *intfdata; + int i; + int left, todo; + struct urb *this_urb = NULL; /* spurious */ + int err; + unsigned long flags; + + portdata = usb_get_serial_port_data(port); + intfdata = usb_get_serial_data(port->serial); + + dev_dbg(&port->dev, "%s: write (%d chars)\n", __func__, count); + + i = 0; + left = count; + for (i = 0; left > 0 && i < N_OUT_URB; i++) { + todo = left; + if (todo > OUT_BUFLEN) + todo = OUT_BUFLEN; + + this_urb = portdata->out_urbs[i]; + if (test_and_set_bit(i, &portdata->out_busy)) { + if (time_before(jiffies, + portdata->tx_start_time[i] + 10 * HZ)) + continue; + usb_unlink_urb(this_urb); + continue; + } + dev_dbg(&port->dev, "%s: endpoint %d buf %d\n", __func__, + usb_pipeendpoint(this_urb->pipe), i); + + err = usb_autopm_get_interface_async(port->serial->interface); + if (err < 0) { + clear_bit(i, &portdata->out_busy); + break; + } + + /* send the data */ + memcpy(this_urb->transfer_buffer, buf, todo); + this_urb->transfer_buffer_length = todo; + + spin_lock_irqsave(&intfdata->susp_lock, flags); + if (intfdata->suspended) { + usb_anchor_urb(this_urb, &portdata->delayed); + spin_unlock_irqrestore(&intfdata->susp_lock, flags); + } else { + intfdata->in_flight++; + spin_unlock_irqrestore(&intfdata->susp_lock, flags); + err = usb_submit_urb(this_urb, GFP_ATOMIC); + if (err) { + dev_err(&port->dev, + "%s: submit urb %d failed: %d\n", + __func__, i, err); + clear_bit(i, &portdata->out_busy); + spin_lock_irqsave(&intfdata->susp_lock, flags); + intfdata->in_flight--; + spin_unlock_irqrestore(&intfdata->susp_lock, + flags); + usb_autopm_put_interface_async(port->serial->interface); + break; + } + } + + portdata->tx_start_time[i] = jiffies; + buf += todo; + left -= todo; + } + + count -= left; + dev_dbg(&port->dev, "%s: wrote (did %d)\n", __func__, count); + return count; +} +EXPORT_SYMBOL(usb_wwan_write); + +static void usb_wwan_indat_callback(struct urb *urb) +{ + int err; + int endpoint; + struct usb_serial_port *port; + struct device *dev; + unsigned char *data = urb->transfer_buffer; + int status = urb->status; + + endpoint = usb_pipeendpoint(urb->pipe); + port = urb->context; + dev = &port->dev; + + if (status) { + dev_dbg(dev, "%s: nonzero status: %d on endpoint %02x.\n", + __func__, status, endpoint); + } else { + if (urb->actual_length) { + tty_insert_flip_string(&port->port, data, + urb->actual_length); + tty_flip_buffer_push(&port->port); + } else + dev_dbg(dev, "%s: empty read urb received\n", __func__); + } + /* Resubmit urb so we continue receiving */ + err = usb_submit_urb(urb, GFP_ATOMIC); + if (err) { + if (err != -EPERM && err != -ENODEV) { + dev_err(dev, "%s: resubmit read urb failed. (%d)\n", + __func__, err); + /* busy also in error unless we are killed */ + usb_mark_last_busy(port->serial->dev); + } + } else { + usb_mark_last_busy(port->serial->dev); + } +} + +static void usb_wwan_outdat_callback(struct urb *urb) +{ + struct usb_serial_port *port; + struct usb_wwan_port_private *portdata; + struct usb_wwan_intf_private *intfdata; + int i; + + port = urb->context; + intfdata = usb_get_serial_data(port->serial); + + usb_serial_port_softint(port); + usb_autopm_put_interface_async(port->serial->interface); + portdata = usb_get_serial_port_data(port); + spin_lock(&intfdata->susp_lock); + intfdata->in_flight--; + spin_unlock(&intfdata->susp_lock); + + for (i = 0; i < N_OUT_URB; ++i) { + if (portdata->out_urbs[i] == urb) { + smp_mb__before_atomic(); + clear_bit(i, &portdata->out_busy); + break; + } + } +} + +int usb_wwan_write_room(struct tty_struct *tty) +{ + struct usb_serial_port *port = tty->driver_data; + struct usb_wwan_port_private *portdata; + int i; + int data_len = 0; + struct urb *this_urb; + + portdata = usb_get_serial_port_data(port); + + for (i = 0; i < N_OUT_URB; i++) { + this_urb = portdata->out_urbs[i]; + if (this_urb && !test_bit(i, &portdata->out_busy)) + data_len += OUT_BUFLEN; + } + + dev_dbg(&port->dev, "%s: %d\n", __func__, data_len); + return data_len; +} +EXPORT_SYMBOL(usb_wwan_write_room); + +int usb_wwan_chars_in_buffer(struct tty_struct *tty) +{ + struct usb_serial_port *port = tty->driver_data; + struct usb_wwan_port_private *portdata; + int i; + int data_len = 0; + struct urb *this_urb; + + portdata = usb_get_serial_port_data(port); + + for (i = 0; i < N_OUT_URB; i++) { + this_urb = portdata->out_urbs[i]; + /* FIXME: This locking is insufficient as this_urb may + go unused during the test */ + if (this_urb && test_bit(i, &portdata->out_busy)) + data_len += this_urb->transfer_buffer_length; + } + dev_dbg(&port->dev, "%s: %d\n", __func__, data_len); + return data_len; +} +EXPORT_SYMBOL(usb_wwan_chars_in_buffer); + +int usb_wwan_open(struct tty_struct *tty, struct usb_serial_port *port) +{ + struct usb_wwan_port_private *portdata; + struct usb_wwan_intf_private *intfdata; + struct usb_serial *serial = port->serial; + int i, err; + struct urb *urb; + + portdata = usb_get_serial_port_data(port); + intfdata = usb_get_serial_data(serial); + + if (port->interrupt_in_urb) { + err = usb_submit_urb(port->interrupt_in_urb, GFP_KERNEL); + if (err) { + dev_err(&port->dev, "%s: submit int urb failed: %d\n", + __func__, err); + } + } + + /* Start reading from the IN endpoint */ + for (i = 0; i < N_IN_URB; i++) { + urb = portdata->in_urbs[i]; + if (!urb) + continue; + err = usb_submit_urb(urb, GFP_KERNEL); + if (err) { + dev_err(&port->dev, + "%s: submit read urb %d failed: %d\n", + __func__, i, err); + } + } + + spin_lock_irq(&intfdata->susp_lock); + if (++intfdata->open_ports == 1) + serial->interface->needs_remote_wakeup = 1; + spin_unlock_irq(&intfdata->susp_lock); + /* this balances a get in the generic USB serial code */ + usb_autopm_put_interface(serial->interface); + + return 0; +} +EXPORT_SYMBOL(usb_wwan_open); + +static void unbusy_queued_urb(struct urb *urb, + struct usb_wwan_port_private *portdata) +{ + int i; + + for (i = 0; i < N_OUT_URB; i++) { + if (urb == portdata->out_urbs[i]) { + clear_bit(i, &portdata->out_busy); + break; + } + } +} + +void usb_wwan_close(struct usb_serial_port *port) +{ + int i; + struct usb_serial *serial = port->serial; + struct usb_wwan_port_private *portdata; + struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial); + struct urb *urb; + + portdata = usb_get_serial_port_data(port); + + /* + * Need to take susp_lock to make sure port is not already being + * resumed, but no need to hold it due to initialized + */ + spin_lock_irq(&intfdata->susp_lock); + if (--intfdata->open_ports == 0) + serial->interface->needs_remote_wakeup = 0; + spin_unlock_irq(&intfdata->susp_lock); + + for (;;) { + urb = usb_get_from_anchor(&portdata->delayed); + if (!urb) + break; + unbusy_queued_urb(urb, portdata); + usb_autopm_put_interface_async(serial->interface); + } + + for (i = 0; i < N_IN_URB; i++) + usb_kill_urb(portdata->in_urbs[i]); + for (i = 0; i < N_OUT_URB; i++) + usb_kill_urb(portdata->out_urbs[i]); + usb_kill_urb(port->interrupt_in_urb); + + usb_autopm_get_interface_no_resume(serial->interface); +} +EXPORT_SYMBOL(usb_wwan_close); + +static struct urb *usb_wwan_setup_urb(struct usb_serial_port *port, + int endpoint, + int dir, void *ctx, char *buf, int len, + void (*callback) (struct urb *)) +{ + struct usb_serial *serial = port->serial; + struct urb *urb; + + urb = usb_alloc_urb(0, GFP_KERNEL); /* No ISO */ + if (!urb) + return NULL; + + usb_fill_bulk_urb(urb, serial->dev, + usb_sndbulkpipe(serial->dev, endpoint) | dir, + buf, len, callback, ctx); + #if 1 //Added by Quectel for Zero Packet + if (dir == USB_DIR_OUT) { + struct usb_device_descriptor *desc = &serial->dev->descriptor; + if (desc->idVendor == cpu_to_le16(0x05C6) && desc->idProduct == cpu_to_le16(0x9090)) + urb->transfer_flags |= URB_ZERO_PACKET; + if (desc->idVendor == cpu_to_le16(0x05C6) && desc->idProduct == cpu_to_le16(0x9003)) + urb->transfer_flags |= URB_ZERO_PACKET; + if (desc->idVendor == cpu_to_le16(0x05C6) && desc->idProduct == cpu_to_le16(0x9215)) + urb->transfer_flags |= URB_ZERO_PACKET; + if (desc->idVendor == cpu_to_le16(0x2C7C)) + urb->transfer_flags |= URB_ZERO_PACKET; + } + #endif + + return urb; +} + +int usb_wwan_port_probe(struct usb_serial_port *port) +{ + struct usb_wwan_port_private *portdata; + struct urb *urb; + u8 *buffer; + int i; + + if (!port->bulk_in_size || !port->bulk_out_size) + return -ENODEV; + + portdata = kzalloc(sizeof(*portdata), GFP_KERNEL); + if (!portdata) + return -ENOMEM; + + init_usb_anchor(&portdata->delayed); + + for (i = 0; i < N_IN_URB; i++) { + buffer = (u8 *)__get_free_page(GFP_KERNEL); + if (!buffer) + goto bail_out_error; + portdata->in_buffer[i] = buffer; + + urb = usb_wwan_setup_urb(port, port->bulk_in_endpointAddress, + USB_DIR_IN, port, + buffer, IN_BUFLEN, + usb_wwan_indat_callback); + portdata->in_urbs[i] = urb; + } + + for (i = 0; i < N_OUT_URB; i++) { + buffer = kmalloc(OUT_BUFLEN, GFP_KERNEL); + if (!buffer) + goto bail_out_error2; + portdata->out_buffer[i] = buffer; + + urb = usb_wwan_setup_urb(port, port->bulk_out_endpointAddress, + USB_DIR_OUT, port, + buffer, OUT_BUFLEN, + usb_wwan_outdat_callback); + portdata->out_urbs[i] = urb; + } + + usb_set_serial_port_data(port, portdata); + + return 0; + +bail_out_error2: + for (i = 0; i < N_OUT_URB; i++) { + usb_free_urb(portdata->out_urbs[i]); + kfree(portdata->out_buffer[i]); + } +bail_out_error: + for (i = 0; i < N_IN_URB; i++) { + usb_free_urb(portdata->in_urbs[i]); + free_page((unsigned long)portdata->in_buffer[i]); + } + kfree(portdata); + + return -ENOMEM; +} +EXPORT_SYMBOL_GPL(usb_wwan_port_probe); + +int usb_wwan_port_remove(struct usb_serial_port *port) +{ + int i; + struct usb_wwan_port_private *portdata; + + portdata = usb_get_serial_port_data(port); + usb_set_serial_port_data(port, NULL); + + for (i = 0; i < N_IN_URB; i++) { + usb_free_urb(portdata->in_urbs[i]); + free_page((unsigned long)portdata->in_buffer[i]); + } + for (i = 0; i < N_OUT_URB; i++) { + usb_free_urb(portdata->out_urbs[i]); + kfree(portdata->out_buffer[i]); + } + + kfree(portdata); + + return 0; +} +EXPORT_SYMBOL(usb_wwan_port_remove); + +#ifdef CONFIG_PM +static void stop_urbs(struct usb_serial *serial) +{ + int i, j; + struct usb_serial_port *port; + struct usb_wwan_port_private *portdata; + + for (i = 0; i < serial->num_ports; ++i) { + port = serial->port[i]; + portdata = usb_get_serial_port_data(port); + if (!portdata) + continue; + for (j = 0; j < N_IN_URB; j++) + usb_kill_urb(portdata->in_urbs[j]); + for (j = 0; j < N_OUT_URB; j++) + usb_kill_urb(portdata->out_urbs[j]); + usb_kill_urb(port->interrupt_in_urb); + } +} + +int usb_wwan_suspend(struct usb_serial *serial, pm_message_t message) +{ + struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial); + + spin_lock_irq(&intfdata->susp_lock); + if (PMSG_IS_AUTO(message)) { + if (intfdata->in_flight) { + spin_unlock_irq(&intfdata->susp_lock); + return -EBUSY; + } + } + intfdata->suspended = 1; + spin_unlock_irq(&intfdata->susp_lock); + + stop_urbs(serial); + + return 0; +} +EXPORT_SYMBOL(usb_wwan_suspend); + +/* Caller must hold susp_lock. */ +static int usb_wwan_submit_delayed_urbs(struct usb_serial_port *port) +{ + struct usb_serial *serial = port->serial; + struct usb_wwan_intf_private *data = usb_get_serial_data(serial); + struct usb_wwan_port_private *portdata; + struct urb *urb; + int err_count = 0; + int err; + + portdata = usb_get_serial_port_data(port); + + for (;;) { + urb = usb_get_from_anchor(&portdata->delayed); + if (!urb) + break; + + err = usb_submit_urb(urb, GFP_ATOMIC); + if (err) { + dev_err(&port->dev, "%s: submit urb failed: %d\n", + __func__, err); + err_count++; + unbusy_queued_urb(urb, portdata); + usb_autopm_put_interface_async(serial->interface); + continue; + } + data->in_flight++; + } + + if (err_count) + return -EIO; + + return 0; +} + +int usb_wwan_resume(struct usb_serial *serial) +{ + int i, j; + struct usb_serial_port *port; + struct usb_wwan_intf_private *intfdata = usb_get_serial_data(serial); + struct usb_wwan_port_private *portdata; + struct urb *urb; + int err; + int err_count = 0; + + spin_lock_irq(&intfdata->susp_lock); + for (i = 0; i < serial->num_ports; i++) { + port = serial->port[i]; + + if (!tty_port_initialized(&port->port)) + continue; + + portdata = usb_get_serial_port_data(port); + + if (port->interrupt_in_urb) { + err = usb_submit_urb(port->interrupt_in_urb, + GFP_ATOMIC); + if (err) { + dev_err(&port->dev, + "%s: submit int urb failed: %d\n", + __func__, err); + err_count++; + } + } + + err = usb_wwan_submit_delayed_urbs(port); + if (err) + err_count++; + + for (j = 0; j < N_IN_URB; j++) { + urb = portdata->in_urbs[j]; + err = usb_submit_urb(urb, GFP_ATOMIC); + if (err < 0) { + dev_err(&port->dev, + "%s: submit read urb %d failed: %d\n", + __func__, i, err); + err_count++; + } + } + } + intfdata->suspended = 0; + spin_unlock_irq(&intfdata->susp_lock); + + if (err_count) + return -EIO; + + return 0; +} +EXPORT_SYMBOL(usb_wwan_resume); +#endif + +MODULE_AUTHOR(DRIVER_AUTHOR); +MODULE_DESCRIPTION(DRIVER_DESC); +MODULE_LICENSE("GPL"); diff --git a/quectel-CM/GobiNetCM.c b/quectel-CM/GobiNetCM.c new file mode 100755 index 0000000..3bd1b94 --- /dev/null +++ b/quectel-CM/GobiNetCM.c @@ -0,0 +1,218 @@ +#include +#include +#include +#include +#include +#include "QMIThread.h" + +#ifdef CONFIG_GOBINET + +// IOCTL to generate a client ID for this service type +#define IOCTL_QMI_GET_SERVICE_FILE 0x8BE0 + 1 + +// IOCTL to get the VIDPID of the device +#define IOCTL_QMI_GET_DEVICE_VIDPID 0x8BE0 + 2 + +// IOCTL to get the MEID of the device +#define IOCTL_QMI_GET_DEVICE_MEID 0x8BE0 + 3 + +int GobiNetSendQMI(PQCQMIMSG pRequest) { + int ret, fd; + + fd = qmiclientId[pRequest->QMIHdr.QMIType]; + + if (fd <= 0) { + dbg_time("%s QMIType: %d has no clientID", __func__, pRequest->QMIHdr.QMIType); + return -ENODEV; + } + + // Always ready to write + if (1 == 1) { + ssize_t nwrites = le16_to_cpu(pRequest->QMIHdr.Length) + 1 - sizeof(QCQMI_HDR); + ret = write(fd, &pRequest->MUXMsg, nwrites); + if (ret == nwrites) { + ret = 0; + } else { + dbg_time("%s write=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + } + } else { + dbg_time("%s poll=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + } + + return ret; +} + +static int GobiNetGetClientID(const char *qcqmi, UCHAR QMIType) { + int ClientId; + ClientId = open(qcqmi, O_RDWR | O_NONBLOCK | O_NOCTTY); + if (ClientId == -1) { + dbg_time("failed to open %s, errno: %d (%s)", qcqmi, errno, strerror(errno)); + return -1; + } + fcntl(cdc_wdm_fd, F_SETFD, FD_CLOEXEC) ; + + if (ioctl(ClientId, IOCTL_QMI_GET_SERVICE_FILE, QMIType) != 0) { + dbg_time("failed to get ClientID for 0x%02x errno: %d (%s)", QMIType, errno, strerror(errno)); + close(ClientId); + ClientId = 0; + } + + switch (QMIType) { + case QMUX_TYPE_WDS: dbg_time("Get clientWDS = %d", ClientId); break; + case QMUX_TYPE_DMS: dbg_time("Get clientDMS = %d", ClientId); break; + case QMUX_TYPE_NAS: dbg_time("Get clientNAS = %d", ClientId); break; + case QMUX_TYPE_QOS: dbg_time("Get clientQOS = %d", ClientId); break; + case QMUX_TYPE_WMS: dbg_time("Get clientWMS = %d", ClientId); break; + case QMUX_TYPE_PDS: dbg_time("Get clientPDS = %d", ClientId); break; + case QMUX_TYPE_UIM: dbg_time("Get clientUIM = %d", ClientId); break; + case QMUX_TYPE_WDS_ADMIN: dbg_time("Get clientWDA = %d", ClientId); + break; + default: break; + } + + return ClientId; +} + +int GobiNetDeInit(void) { + unsigned int i; + for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++) + { + if (qmiclientId[i] != 0) + { + close(qmiclientId[i]); + qmiclientId[i] = 0; + } + } + + return 0; +} + +void * GobiNetThread(void *pData) { + PROFILE_T *profile = (PROFILE_T *)pData; + const char *qcqmi = (const char *)profile->qmichannel; + + qmiclientId[QMUX_TYPE_WDS] = GobiNetGetClientID(qcqmi, QMUX_TYPE_WDS); + if (profile->IsDualIPSupported) + qmiclientId[QMUX_TYPE_WDS_IPV6] = GobiNetGetClientID(qcqmi, QMUX_TYPE_WDS); + qmiclientId[QMUX_TYPE_DMS] = GobiNetGetClientID(qcqmi, QMUX_TYPE_DMS); + qmiclientId[QMUX_TYPE_NAS] = GobiNetGetClientID(qcqmi, QMUX_TYPE_NAS); + qmiclientId[QMUX_TYPE_UIM] = GobiNetGetClientID(qcqmi, QMUX_TYPE_UIM); + if (profile->qmapnet_adapter == NULL) //when QMAP enabled, set data format in GobiNet Driver + qmiclientId[QMUX_TYPE_WDS_ADMIN] = GobiNetGetClientID(qcqmi, QMUX_TYPE_WDS_ADMIN); + + //donot check clientWDA, there is only one client for WDA, if quectel-CM is killed by SIGKILL, i cannot get client ID for WDA again! + if (qmiclientId[QMUX_TYPE_WDS] == 0) /*|| (clientWDA == -1)*/ { + GobiNetDeInit(); + dbg_time("%s Failed to open %s, errno: %d (%s)", __func__, qcqmi, errno, strerror(errno)); + qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_DISCONNECTED); + pthread_exit(NULL); + return NULL; + } + + qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_CONNECTED); + + while (1) { + struct pollfd pollfds[16] = {{qmidevice_control_fd[1], POLLIN, 0}}; + int ne, ret, nevents = 1; + unsigned int i; + + for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++) + { + if (qmiclientId[i] != 0) + { + pollfds[nevents].fd = qmiclientId[i]; + pollfds[nevents].events = POLLIN; + pollfds[nevents].revents = 0; + nevents++; + } + } + + do { + ret = poll(pollfds, nevents, -1); + } while ((ret < 0) && (errno == EINTR)); + + if (ret <= 0) { + dbg_time("%s poll=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + break; + } + + for (ne = 0; ne < nevents; ne++) { + int fd = pollfds[ne].fd; + short revents = pollfds[ne].revents; + + if (revents & (POLLERR | POLLHUP | POLLNVAL)) { + dbg_time("%s poll err/hup/inval", __func__); + dbg_time("epoll fd = %d, events = 0x%04x", fd, revents); + if (fd == qmidevice_control_fd[1]) { + } else { + } + if (revents & (POLLERR | POLLHUP | POLLNVAL)) + goto __GobiNetThread_quit; + } + + if ((revents & POLLIN) == 0) + continue; + + if (fd == qmidevice_control_fd[1]) { + int triger_event; + if (read(fd, &triger_event, sizeof(triger_event)) == sizeof(triger_event)) { + //DBG("triger_event = 0x%x", triger_event); + switch (triger_event) { + case RIL_REQUEST_QUIT: + goto __GobiNetThread_quit; + break; + case SIGTERM: + case SIGHUP: + case SIGINT: + QmiThreadRecvQMI(NULL); + break; + default: + break; + } + } + continue; + } + + { + ssize_t nreads; + UCHAR QMIBuf[512]; + PQCQMIMSG pResponse = (PQCQMIMSG)QMIBuf; + + nreads = read(fd, &pResponse->MUXMsg, sizeof(QMIBuf) - sizeof(QCQMI_HDR)); + if (nreads <= 0) + { + dbg_time("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno)); + break; + } + + for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++) + { + if (qmiclientId[i] == fd) + { + pResponse->QMIHdr.QMIType = i; + } + } + + pResponse->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; + pResponse->QMIHdr.Length = cpu_to_le16(nreads + sizeof(QCQMI_HDR) - 1); + pResponse->QMIHdr.CtlFlags = 0x00; + pResponse->QMIHdr.ClientId = fd & 0xFF; + + QmiThreadRecvQMI(pResponse); + } + } + } + +__GobiNetThread_quit: + GobiNetDeInit(); + qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_DISCONNECTED); + QmiThreadRecvQMI(NULL); //main thread may pending on QmiThreadSendQMI() + dbg_time("%s exit", __func__); + pthread_exit(NULL); + return NULL; +} + +#else +int GobiNetSendQMI(PQCQMIMSG pRequest) {return -1;} +void * GobiNetThread(void *pData) {dbg_time("please set CONFIG_GOBINET"); return NULL;} +#endif diff --git a/quectel-CM/MPQCTL.h b/quectel-CM/MPQCTL.h new file mode 100755 index 0000000..7b43a6e --- /dev/null +++ b/quectel-CM/MPQCTL.h @@ -0,0 +1,376 @@ +/*=========================================================================== + + M P Q C T L. H +DESCRIPTION: + + This module contains QMI QCTL module. + +INITIALIZATION AND SEQUENCING REQUIREMENTS: + +Copyright (C) 2011 by Qualcomm Technologies, Incorporated. All Rights Reserved. +===========================================================================*/ + +#ifndef MPQCTL_H +#define MPQCTL_H + +#include "MPQMI.h" + +#pragma pack(push, 1) + +// ================= QMICTL ================== + +// QMICTL Control Flags +#define QMICTL_CTL_FLAG_CMD 0x00 +#define QMICTL_CTL_FLAG_RSP 0x01 +#define QMICTL_CTL_FLAG_IND 0x02 + +#if 0 +typedef struct _QMICTL_TRANSACTION_ITEM +{ + LIST_ENTRY List; + UCHAR TransactionId; // QMICTL transaction id + PVOID Context; // Adapter or IocDev + PIRP Irp; +} QMICTL_TRANSACTION_ITEM, *PQMICTL_TRANSACTION_ITEM; +#endif + +typedef struct _QCQMICTL_MSG_HDR +{ + UCHAR CtlFlags; // 00-cmd, 01-rsp, 10-ind + UCHAR TransactionId; + USHORT QMICTLType; + USHORT Length; +} __attribute__ ((packed)) QCQMICTL_MSG_HDR, *PQCQMICTL_MSG_HDR; + +#define QCQMICTL_MSG_HDR_SIZE sizeof(QCQMICTL_MSG_HDR) + +typedef struct _QCQMICTL_MSG_HDR_RESP +{ + UCHAR CtlFlags; // 00-cmd, 01-rsp, 10-ind + UCHAR TransactionId; + USHORT QMICTLType; + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} __attribute__ ((packed)) QCQMICTL_MSG_HDR_RESP, *PQCQMICTL_MSG_HDR_RESP; + +typedef struct _QCQMICTL_MSG +{ + UCHAR CtlFlags; // 00-cmd, 01-rsp, 10-ind + UCHAR TransactionId; + USHORT QMICTLType; + USHORT Length; + UCHAR Payload; +} __attribute__ ((packed)) QCQMICTL_MSG, *PQCQMICTL_MSG; + +// TLV Header +typedef struct _QCQMICTL_TLV_HDR +{ + UCHAR TLVType; + USHORT TLVLength; +} __attribute__ ((packed)) QCQMICTL_TLV_HDR, *PQCQMICTL_TLV_HDR; + +#define QCQMICTL_TLV_HDR_SIZE sizeof(QCQMICTL_TLV_HDR) + +// QMICTL Type +#define QMICTL_SET_INSTANCE_ID_REQ 0x0020 +#define QMICTL_SET_INSTANCE_ID_RESP 0x0020 +#define QMICTL_GET_VERSION_REQ 0x0021 +#define QMICTL_GET_VERSION_RESP 0x0021 +#define QMICTL_GET_CLIENT_ID_REQ 0x0022 +#define QMICTL_GET_CLIENT_ID_RESP 0x0022 +#define QMICTL_RELEASE_CLIENT_ID_REQ 0x0023 +#define QMICTL_RELEASE_CLIENT_ID_RESP 0x0023 +#define QMICTL_REVOKE_CLIENT_ID_IND 0x0024 +#define QMICTL_INVALID_CLIENT_ID_IND 0x0025 +#define QMICTL_SET_DATA_FORMAT_REQ 0x0026 +#define QMICTL_SET_DATA_FORMAT_RESP 0x0026 +#define QMICTL_SYNC_REQ 0x0027 +#define QMICTL_SYNC_RESP 0x0027 +#define QMICTL_SYNC_IND 0x0027 + +#define QMICTL_FLAG_REQUEST 0x00 +#define QMICTL_FLAG_RESPONSE 0x01 +#define QMICTL_FLAG_INDICATION 0x02 + +// QMICTL Message Definitions + +typedef struct _QMICTL_SET_INSTANCE_ID_REQ_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_REQUEST + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_SET_INSTANCE_ID_REQ + USHORT Length; // 4 + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // 1 + UCHAR Value; // Host-unique QMI instance for this device driver +} __attribute__ ((packed)) QMICTL_SET_INSTANCE_ID_REQ_MSG, *PQMICTL_SET_INSTANCE_ID_REQ_MSG; + +typedef struct _QMICTL_SET_INSTANCE_ID_RESP_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_RESPONSE + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_SET_INSTANCE_ID_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMIResult; + USHORT QMIError; + UCHAR TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLV2Length; // 0x0002 + USHORT QMI_ID; // Upper byte is assigned by MSM, + // lower assigned by host +} __attribute__ ((packed)) QMICTL_SET_INSTANCE_ID_RESP_MSG, *PQMICTL_SET_INSTANCE_ID_RESP_MSG; + +typedef struct _QMICTL_GET_VERSION_REQ_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_REQUEST + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_GET_VERSION_REQ + USHORT Length; // 0 + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // var + UCHAR QMUXTypes; // List of one byte QMUX_TYPE values + // 0xFF returns a list of versions for all + // QMUX_TYPEs implemented on the device +} __attribute__ ((packed)) QMICTL_GET_VERSION_REQ_MSG, *PQMICTL_GET_VERSION_REQ_MSG; + +typedef struct _QMUX_TYPE_VERSION_STRUCT +{ + UCHAR QMUXType; + USHORT MajorVersion; + USHORT MinorVersion; +} __attribute__ ((packed)) QMUX_TYPE_VERSION_STRUCT, *PQMUX_TYPE_VERSION_STRUCT; + +typedef struct _ADDENDUM_VERSION_PREAMBLE +{ + UCHAR LabelLength; + UCHAR Label; +} __attribute__ ((packed)) ADDENDUM_VERSION_PREAMBLE, *PADDENDUM_VERSION_PREAMBLE; + +#define QMICTL_GET_VERSION_RSP_TLV_TYPE_VERSION 0x01 +#define QMICTL_GET_VERSION_RSP_TLV_TYPE_ADD_VERSION 0x10 + +typedef struct _QMICTL_GET_VERSION_RESP_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_RESPONSE + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_GET_VERSION_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMIResult; + USHORT QMIError; + UCHAR TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLV2Length; // var + UCHAR NumElements; // Num of QMUX_TYPE_VERSION_STRUCT + QMUX_TYPE_VERSION_STRUCT TypeVersion[0]; +} __attribute__ ((packed)) QMICTL_GET_VERSION_RESP_MSG, *PQMICTL_GET_VERSION_RESP_MSG; + +typedef struct _QMICTL_GET_CLIENT_ID_REQ_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_REQUEST + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_GET_CLIENT_ID_REQ + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // 1 + UCHAR QMIType; // QMUX type +} __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_REQ_MSG, *PQMICTL_GET_CLIENT_ID_REQ_MSG; + +typedef struct _QMICTL_GET_CLIENT_ID_RESP_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_RESPONSE + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_GET_CLIENT_ID_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMIResult; // result code + USHORT QMIError; // error code + UCHAR TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLV2Length; // 2 + UCHAR QMIType; + UCHAR ClientId; +} __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_RESP_MSG, *PQMICTL_GET_CLIENT_ID_RESP_MSG; + +typedef struct _QMICTL_RELEASE_CLIENT_ID_REQ_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_REQUEST + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_RELEASE_CLIENT_ID_REQ + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // 0x0002 + UCHAR QMIType; + UCHAR ClientId; +} __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_REQ_MSG, *PQMICTL_RELEASE_CLIENT_ID_REQ_MSG; + +typedef struct _QMICTL_RELEASE_CLIENT_ID_RESP_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_RESPONSE + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_RELEASE_CLIENT_ID_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMIResult; // result code + USHORT QMIError; // error code + UCHAR TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLV2Length; // 2 + UCHAR QMIType; + UCHAR ClientId; +} __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_RESP_MSG, *PQMICTL_RELEASE_CLIENT_ID_RESP_MSG; + +typedef struct _QMICTL_REVOKE_CLIENT_ID_IND_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_INDICATION + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_REVOKE_CLIENT_ID_IND + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // 0x0002 + UCHAR QMIType; + UCHAR ClientId; +} __attribute__ ((packed)) QMICTL_REVOKE_CLIENT_ID_IND_MSG, *PQMICTL_REVOKE_CLIENT_ID_IND_MSG; + +typedef struct _QMICTL_INVALID_CLIENT_ID_IND_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_INDICATION + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_REVOKE_CLIENT_ID_IND + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // 0x0002 + UCHAR QMIType; + UCHAR ClientId; +} __attribute__ ((packed)) QMICTL_INVALID_CLIENT_ID_IND_MSG, *PQMICTL_INVALID_CLIENT_ID_IND_MSG; + +typedef struct _QMICTL_SET_DATA_FORMAT_REQ_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_REQUEST + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_SET_DATA_FORMAT_REQ + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + USHORT TLVLength; // 1 + UCHAR DataFormat; // 0-default; 1-QoS hdr present +} __attribute__ ((packed)) QMICTL_SET_DATA_FORMAT_REQ_MSG, *PQMICTL_SET_DATA_FORMAT_REQ_MSG; + +#ifdef QC_IP_MODE +#define SET_DATA_FORMAT_TLV_TYPE_LINK_PROTO 0x10 +#define SET_DATA_FORMAT_LINK_PROTO_ETH 0x0001 +#define SET_DATA_FORMAT_LINK_PROTO_IP 0x0002 +typedef struct _QMICTL_SET_DATA_FORMAT_TLV_LINK_PROT +{ + UCHAR TLVType; // Link-Layer Protocol + USHORT TLVLength; // 2 + USHORT LinkProt; // 0x1: ETH; 0x2: IP +} QMICTL_SET_DATA_FORMAT_TLV_LINK_PROT, *PQMICTL_SET_DATA_FORMAT_TLV_LINK_PROT; + +#ifdef QCMP_UL_TLP +#define SET_DATA_FORMAT_TLV_TYPE_UL_TLP 0x11 +typedef struct _QMICTL_SET_DATA_FORMAT_TLV_UL_TLP +{ + UCHAR TLVType; // 0x11, Uplink TLP Setting + USHORT TLVLength; // 1 + UCHAR UlTlpSetting; // 0x0: Disable; 0x01: Enable +} QMICTL_SET_DATA_FORMAT_TLV_UL_TLP, *PQMICTL_SET_DATA_FORMAT_TLV_UL_TLP; +#endif // QCMP_UL_TLP + +#ifdef QCMP_DL_TLP +#define SET_DATA_FORMAT_TLV_TYPE_DL_TLP 0x13 +typedef struct _QMICTL_SET_DATA_FORMAT_TLV_DL_TLP +{ + UCHAR TLVType; // 0x11, Uplink TLP Setting + USHORT TLVLength; // 1 + UCHAR DlTlpSetting; // 0x0: Disable; 0x01: Enable +} QMICTL_SET_DATA_FORMAT_TLV_DL_TLP, *PQMICTL_SET_DATA_FORMAT_TLV_DL_TLP; +#endif // QCMP_DL_TLP + +#endif // QC_IP_MODE + +#ifdef MP_QCQOS_ENABLED +#define SET_DATA_FORMAT_TLV_TYPE_QOS_SETTING 0x12 +typedef struct _QMICTL_SET_DATA_FORMAT_TLV_QOS_SETTING +{ + UCHAR TLVType; // 0x12, QoS setting + USHORT TLVLength; // 1 + UCHAR QosSetting; // 0x0: Disable; 0x01: Enable +} QMICTL_SET_DATA_FORMAT_TLV_QOS_SETTING, *PQMICTL_SET_DATA_FORMAT_TLV_QOS_SETTING; +#endif // MP_QCQOS_ENABLED + +typedef struct _QMICTL_SET_DATA_FORMAT_RESP_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_RESPONSE + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_SET_DATA_FORMAT_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMIResult; // result code + USHORT QMIError; // error code +} __attribute__ ((packed)) QMICTL_SET_DATA_FORMAT_RESP_MSG, *PQMICTL_SET_DATA_FORMAT_RESP_MSG; + +typedef struct _QMICTL_SYNC_REQ_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_REQUEST + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_CTL_SYNC_REQ + USHORT Length; // 0 +} __attribute__ ((packed)) QMICTL_SYNC_REQ_MSG, *PQMICTL_SYNC_REQ_MSG; + +typedef struct _QMICTL_SYNC_RESP_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_RESPONSE + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_CTL_SYNC_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMIResult; + USHORT QMIError; +} __attribute__ ((packed)) QMICTL_SYNC_RESP_MSG, *PQMICTL_SYNC_RESP_MSG; + +typedef struct _QMICTL_SYNC_IND_MSG +{ + UCHAR CtlFlags; // QMICTL_FLAG_INDICATION + UCHAR TransactionId; + USHORT QMICTLType; // QMICTL_REVOKE_CLIENT_ID_IND + USHORT Length; +} __attribute__ ((packed)) QMICTL_SYNC_IND_MSG, *PQMICTL_SYNC_IND_MSG; + +typedef struct _QMICTL_MSG +{ + union + { + // Message Header + QCQMICTL_MSG_HDR QMICTLMsgHdr; + QCQMICTL_MSG_HDR_RESP QMICTLMsgHdrRsp; + + // QMICTL Message + QMICTL_SET_INSTANCE_ID_REQ_MSG SetInstanceIdReq; + QMICTL_SET_INSTANCE_ID_RESP_MSG SetInstanceIdRsp; + QMICTL_GET_VERSION_REQ_MSG GetVersionReq; + QMICTL_GET_VERSION_RESP_MSG GetVersionRsp; + QMICTL_GET_CLIENT_ID_REQ_MSG GetClientIdReq; + QMICTL_GET_CLIENT_ID_RESP_MSG GetClientIdRsp; + QMICTL_RELEASE_CLIENT_ID_REQ_MSG ReleaseClientIdReq; + QMICTL_RELEASE_CLIENT_ID_RESP_MSG ReleaseClientIdRsp; + QMICTL_REVOKE_CLIENT_ID_IND_MSG RevokeClientIdInd; + QMICTL_INVALID_CLIENT_ID_IND_MSG InvalidClientIdInd; + QMICTL_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq; + QMICTL_SET_DATA_FORMAT_RESP_MSG SetDataFormatRsp; + QMICTL_SYNC_REQ_MSG SyncReq; + QMICTL_SYNC_RESP_MSG SyncRsp; + QMICTL_SYNC_IND_MSG SyncInd; + }; +} __attribute__ ((packed)) QMICTL_MSG, *PQMICTL_MSG; + +#endif // MPQCTL_H diff --git a/quectel-CM/MPQMI.h b/quectel-CM/MPQMI.h new file mode 100755 index 0000000..72ff06d --- /dev/null +++ b/quectel-CM/MPQMI.h @@ -0,0 +1,222 @@ +/*=========================================================================== + + M P Q M I. H +DESCRIPTION: + + This module contains forward references to the QMI module. + +INITIALIZATION AND SEQUENCING REQUIREMENTS: + +Copyright (C) 2011 by Qualcomm Technologies, Incorporated. All Rights Reserved. +===========================================================================*/ +/*=========================================================================== + + EDIT HISTORY FOR FILE + $Header: //depot/QMI/win/qcdrivers/ndis/MPQMI.h#3 $ + +when who what, where, why +-------- --- ---------------------------------------------------------- +11/20/04 hg Initial version. +===========================================================================*/ + +#ifndef USBQMI_H +#define USBQMI_H + +typedef char CHAR; +typedef unsigned char UCHAR; +typedef unsigned short USHORT; +typedef int INT; +typedef unsigned int UINT; +typedef long LONG; +typedef unsigned int ULONG; +typedef unsigned long long ULONG64; +typedef char *PCHAR; +typedef unsigned char *PUCHAR; +typedef int *PINT; +typedef int BOOL; + +#define TRUE (1 == 1) +#define FALSE (1 != 1) + +#define QMICTL_SUPPORTED_MAJOR_VERSION 1 +#define QMICTL_SUPPORTED_MINOR_VERSION 0 + +#pragma pack(push, 1) + +// ========= USB Control Message ========== + +#define USB_CTL_MSG_TYPE_QMI 0x01 + +// USB Control Message +typedef struct _QCUSB_CTL_MSG_HDR +{ + UCHAR IFType; +} __attribute__ ((packed)) QCUSB_CTL_MSG_HDR, *PQCUSB_CTL_MSG_HDR; + +#define QCUSB_CTL_MSG_HDR_SIZE sizeof(QCUSB_CTL_MSG_HDR) + +typedef struct _QCUSB_CTL_MSG +{ + UCHAR IFType; + UCHAR Message; +} __attribute__ ((packed)) QCUSB_CTL_MSG, *PQCUSB_CTL_MSG; + +#define QCTLV_TYPE_REQUIRED_PARAMETER 0x01 +#define QCTLV_TYPE_RESULT_CODE 0x02 + +// ================= QMI ================== + +// Define QMI Type +typedef enum _QMI_SERVICE_TYPE +{ + QMUX_TYPE_CTL = 0x00, + QMUX_TYPE_WDS = 0x01, + QMUX_TYPE_DMS = 0x02, + QMUX_TYPE_NAS = 0x03, + QMUX_TYPE_QOS = 0x04, + QMUX_TYPE_WMS = 0x05, + QMUX_TYPE_PDS = 0x06, + QMUX_TYPE_UIM = 0x0B, + QMUX_TYPE_WDS_IPV6 = 0x11, + QMUX_TYPE_WDS_ADMIN = 0x1A, + QMUX_TYPE_MAX = 0xFF, + QMUX_TYPE_ALL = 0xFF +} QMI_SERVICE_TYPE; + +typedef enum _QMI_RESULT_CODE_TYPE +{ + QMI_RESULT_SUCCESS = 0x0000, + QMI_RESULT_FAILURE = 0x0001 +} QMI_RESULT_CODE_TYPE; + +typedef enum _QMI_ERROR_CODE_TYPE +{ + QMI_ERR_NONE = 0x0000, + QMI_ERR_INTERNAL = 0x0003, + QMI_ERR_CLIENT_IDS_EXHAUSTED = 0x0005, + QMI_ERR_DENIED = 0x0006, + QMI_ERR_INVALID_CLIENT_IDS = 0x0007, + QMI_ERR_NO_BATTERY = 0x0008, + QMI_ERR_INVALID_HANDLE = 0x0009, + QMI_ERR_INVALID_PROFILE = 0x000A, + QMI_ERR_STORAGE_EXCEEDED = 0x000B, + QMI_ERR_INCORRECT_PIN = 0x000C, + QMI_ERR_NO_NETWORK = 0x000D, + QMI_ERR_PIN_LOCKED = 0x000E, + QMI_ERR_OUT_OF_CALL = 0x000F, + QMI_ERR_NOT_PROVISIONED = 0x0010, + QMI_ERR_ARG_TOO_LONG = 0x0013, + QMI_ERR_DEVICE_IN_USE = 0x0017, + QMI_ERR_OP_DEVICE_UNSUPPORTED = 0x0019, + QMI_ERR_NO_EFFECT = 0x001A, + QMI_ERR_INVALID_REGISTER_ACTION = 0x0020, + QMI_ERR_NO_MEMORY = 0x0021, + QMI_ERR_PIN_BLOCKED = 0x0023, + QMI_ERR_PIN_PERM_BLOCKED = 0x0024, + QMI_ERR_INVALID_ARG = 0x0030, + QMI_ERR_INVALID_INDEX = 0x0031, + QMI_ERR_NO_ENTRY = 0x0032, + QMI_ERR_EXTENDED_INTERNAL = 0x0051, + QMI_ERR_ACCESS_DENIED = 0x0052 +} QMI_ERROR_CODE_TYPE; + +#define QCQMI_CTL_FLAG_SERVICE 0x80 +#define QCQMI_CTL_FLAG_CTL_POINT 0x00 + +typedef struct _QCQMI_HDR +{ + UCHAR IFType; + USHORT Length; + UCHAR CtlFlags; // reserved + UCHAR QMIType; + UCHAR ClientId; +} __attribute__ ((packed)) QCQMI_HDR, *PQCQMI_HDR; + +#define QCQMI_HDR_SIZE (sizeof(QCQMI_HDR)-1) + +typedef struct _QCQMI +{ + UCHAR IFType; + USHORT Length; + UCHAR CtlFlags; // reserved + UCHAR QMIType; + UCHAR ClientId; + UCHAR SDU; +} __attribute__ ((packed)) QCQMI, *PQCQMI; + +typedef struct _QMI_SERVICE_VERSION +{ + USHORT Major; + USHORT Minor; + USHORT AddendumMajor; + USHORT AddendumMinor; +} __attribute__ ((packed)) QMI_SERVICE_VERSION, *PQMI_SERVICE_VERSION; + +// ================= QMUX ================== + +#define QMUX_MSG_OVERHEAD_BYTES 4 // Type(USHORT) Length(USHORT) -- header + +#define QMUX_BROADCAST_CID 0xFF + +typedef struct _QCQMUX_HDR +{ + UCHAR CtlFlags; // 0: single QMUX Msg; 1: + USHORT TransactionId; +} __attribute__ ((packed)) QCQMUX_HDR, *PQCQMUX_HDR; + +typedef struct _QCQMUX +{ + UCHAR CtlFlags; // 0: single QMUX Msg; 1: + USHORT TransactionId; + UCHAR Message; // Type(2), Length(2), Value +} __attribute__ ((packed)) QCQMUX, *PQCQMUX; + +#define QCQMUX_HDR_SIZE sizeof(QCQMUX_HDR) + +typedef struct _QCQMUX_MSG_HDR +{ + USHORT Type; + USHORT Length; +} __attribute__ ((packed)) QCQMUX_MSG_HDR, *PQCQMUX_MSG_HDR; + +#define QCQMUX_MSG_HDR_SIZE sizeof(QCQMUX_MSG_HDR) + +typedef struct _QCQMUX_MSG_HDR_RESP +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} __attribute__ ((packed)) QCQMUX_MSG_HDR_RESP, *PQCQMUX_MSG_HDR_RESP; + +typedef struct _QCQMUX_TLV +{ + UCHAR Type; + USHORT Length; + UCHAR Value; +} __attribute__ ((packed)) QCQMUX_TLV, *PQCQMUX_TLV; + +typedef struct _QMI_TLV_HDR +{ + UCHAR TLVType; + USHORT TLVLength; +} __attribute__ ((packed)) QMI_TLV_HDR, *PQMI_TLV_HDR; + +// QMUX Message Definitions -- QMI SDU +#define QMUX_CTL_FLAG_SINGLE_MSG 0x00 +#define QMUX_CTL_FLAG_COMPOUND_MSG 0x01 +#define QMUX_CTL_FLAG_TYPE_CMD 0x00 +#define QMUX_CTL_FLAG_TYPE_RSP 0x02 +#define QMUX_CTL_FLAG_TYPE_IND 0x04 +#define QMUX_CTL_FLAG_MASK_COMPOUND 0x01 +#define QMUX_CTL_FLAG_MASK_TYPE 0x06 // 00-cmd, 01-rsp, 10-ind + +#pragma pack(pop) + +#endif // USBQMI_H diff --git a/quectel-CM/MPQMUX.c b/quectel-CM/MPQMUX.c new file mode 100755 index 0000000..178208a --- /dev/null +++ b/quectel-CM/MPQMUX.c @@ -0,0 +1,424 @@ +#include "QMIThread.h" +static char line[1024]; +static pthread_mutex_t dumpQMIMutex = PTHREAD_MUTEX_INITIALIZER; +#undef dbg +#define dbg( format, arg... ) do {if (strlen(line) < sizeof(line)) snprintf(&line[strlen(line)], sizeof(line) - strlen(line), format, ## arg);} while (0) + +PQMI_TLV_HDR GetTLV (PQCQMUX_MSG_HDR pQMUXMsgHdr, int TLVType); + +typedef struct { + UINT type; + const char *name; +} QMI_NAME_T; + +#define qmi_name_item(type) {type, #type} + +static const QMI_NAME_T qmi_IFType[] = { +{USB_CTL_MSG_TYPE_QMI, "USB_CTL_MSG_TYPE_QMI"}, +}; + +static const QMI_NAME_T qmi_CtlFlags[] = { +qmi_name_item(QMICTL_CTL_FLAG_CMD), +qmi_name_item(QCQMI_CTL_FLAG_SERVICE), +}; + +static const QMI_NAME_T qmi_QMIType[] = { +qmi_name_item(QMUX_TYPE_CTL), +qmi_name_item(QMUX_TYPE_WDS), +qmi_name_item(QMUX_TYPE_DMS), +qmi_name_item(QMUX_TYPE_NAS), +qmi_name_item(QMUX_TYPE_QOS), +qmi_name_item(QMUX_TYPE_WMS), +qmi_name_item(QMUX_TYPE_PDS), +qmi_name_item(QMUX_TYPE_WDS_ADMIN), +}; + +static const QMI_NAME_T qmi_ctl_CtlFlags[] = { +qmi_name_item(QMICTL_FLAG_REQUEST), +qmi_name_item(QMICTL_FLAG_RESPONSE), +qmi_name_item(QMICTL_FLAG_INDICATION), +}; + +static const QMI_NAME_T qmux_ctl_QMICTLType[] = { +// QMICTL Type +qmi_name_item(QMICTL_SET_INSTANCE_ID_REQ), // 0x0020 +qmi_name_item(QMICTL_SET_INSTANCE_ID_RESP), // 0x0020 +qmi_name_item(QMICTL_GET_VERSION_REQ), // 0x0021 +qmi_name_item(QMICTL_GET_VERSION_RESP), // 0x0021 +qmi_name_item(QMICTL_GET_CLIENT_ID_REQ), // 0x0022 +qmi_name_item(QMICTL_GET_CLIENT_ID_RESP), // 0x0022 +qmi_name_item(QMICTL_RELEASE_CLIENT_ID_REQ), // 0x0023 +qmi_name_item(QMICTL_RELEASE_CLIENT_ID_RESP), // 0x0023 +qmi_name_item(QMICTL_REVOKE_CLIENT_ID_IND), // 0x0024 +qmi_name_item(QMICTL_INVALID_CLIENT_ID_IND), // 0x0025 +qmi_name_item(QMICTL_SET_DATA_FORMAT_REQ), // 0x0026 +qmi_name_item(QMICTL_SET_DATA_FORMAT_RESP), // 0x0026 +qmi_name_item(QMICTL_SYNC_REQ), // 0x0027 +qmi_name_item(QMICTL_SYNC_RESP), // 0x0027 +qmi_name_item(QMICTL_SYNC_IND), // 0x0027 +}; + +static const QMI_NAME_T qmux_CtlFlags[] = { +qmi_name_item(QMUX_CTL_FLAG_TYPE_CMD), +qmi_name_item(QMUX_CTL_FLAG_TYPE_RSP), +qmi_name_item(QMUX_CTL_FLAG_TYPE_IND), +}; + + +static const QMI_NAME_T qmux_wds_Type[] = { +qmi_name_item(QMIWDS_SET_EVENT_REPORT_REQ), // 0x0001 +qmi_name_item(QMIWDS_SET_EVENT_REPORT_RESP), // 0x0001 +qmi_name_item(QMIWDS_EVENT_REPORT_IND), // 0x0001 +qmi_name_item(QMIWDS_START_NETWORK_INTERFACE_REQ), // 0x0020 +qmi_name_item(QMIWDS_START_NETWORK_INTERFACE_RESP), // 0x0020 +qmi_name_item(QMIWDS_STOP_NETWORK_INTERFACE_REQ), // 0x0021 +qmi_name_item(QMIWDS_STOP_NETWORK_INTERFACE_RESP), // 0x0021 +qmi_name_item(QMIWDS_GET_PKT_SRVC_STATUS_REQ), // 0x0022 +qmi_name_item(QMIWDS_GET_PKT_SRVC_STATUS_RESP), // 0x0022 +qmi_name_item(QMIWDS_GET_PKT_SRVC_STATUS_IND), // 0x0022 +qmi_name_item(QMIWDS_GET_CURRENT_CHANNEL_RATE_REQ), // 0x0023 +qmi_name_item(QMIWDS_GET_CURRENT_CHANNEL_RATE_RESP), // 0x0023 +qmi_name_item(QMIWDS_GET_PKT_STATISTICS_REQ), // 0x0024 +qmi_name_item(QMIWDS_GET_PKT_STATISTICS_RESP), // 0x0024 +qmi_name_item(QMIWDS_MODIFY_PROFILE_SETTINGS_REQ), // 0x0028 +qmi_name_item(QMIWDS_MODIFY_PROFILE_SETTINGS_RESP), // 0x0028 +qmi_name_item(QMIWDS_GET_PROFILE_SETTINGS_REQ), // 0x002B +qmi_name_item(QMIWDS_GET_PROFILE_SETTINGS_RESP), // 0x002BD +qmi_name_item(QMIWDS_GET_DEFAULT_SETTINGS_REQ), // 0x002C +qmi_name_item(QMIWDS_GET_DEFAULT_SETTINGS_RESP), // 0x002C +qmi_name_item(QMIWDS_GET_RUNTIME_SETTINGS_REQ), // 0x002D +qmi_name_item(QMIWDS_GET_RUNTIME_SETTINGS_RESP), // 0x002D +qmi_name_item(QMIWDS_GET_MIP_MODE_REQ), // 0x002F +qmi_name_item(QMIWDS_GET_MIP_MODE_RESP), // 0x002F +qmi_name_item(QMIWDS_GET_DATA_BEARER_REQ), // 0x0037 +qmi_name_item(QMIWDS_GET_DATA_BEARER_RESP), // 0x0037 +qmi_name_item(QMIWDS_DUN_CALL_INFO_REQ), // 0x0038 +qmi_name_item(QMIWDS_DUN_CALL_INFO_RESP), // 0x0038 +qmi_name_item(QMIWDS_DUN_CALL_INFO_IND), // 0x0038 +qmi_name_item(QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ), // 0x004D +qmi_name_item(QMIWDS_SET_CLIENT_IP_FAMILY_PREF_RESP), // 0x004D +qmi_name_item(QMIWDS_SET_AUTO_CONNECT_REQ), // 0x0051 +qmi_name_item(QMIWDS_SET_AUTO_CONNECT_RESP), // 0x0051 +qmi_name_item(QMIWDS_BIND_MUX_DATA_PORT_REQ), // 0x00A2 +qmi_name_item(QMIWDS_BIND_MUX_DATA_PORT_RESP), // 0x00A2 +}; + +static const QMI_NAME_T qmux_dms_Type[] = { +// ======================= DMS ============================== +qmi_name_item(QMIDMS_SET_EVENT_REPORT_REQ), // 0x0001 +qmi_name_item(QMIDMS_SET_EVENT_REPORT_RESP), // 0x0001 +qmi_name_item(QMIDMS_EVENT_REPORT_IND), // 0x0001 +qmi_name_item(QMIDMS_GET_DEVICE_CAP_REQ), // 0x0020 +qmi_name_item(QMIDMS_GET_DEVICE_CAP_RESP), // 0x0020 +qmi_name_item(QMIDMS_GET_DEVICE_MFR_REQ), // 0x0021 +qmi_name_item(QMIDMS_GET_DEVICE_MFR_RESP), // 0x0021 +qmi_name_item(QMIDMS_GET_DEVICE_MODEL_ID_REQ), // 0x0022 +qmi_name_item(QMIDMS_GET_DEVICE_MODEL_ID_RESP), // 0x0022 +qmi_name_item(QMIDMS_GET_DEVICE_REV_ID_REQ), // 0x0023 +qmi_name_item(QMIDMS_GET_DEVICE_REV_ID_RESP), // 0x0023 +qmi_name_item(QMIDMS_GET_MSISDN_REQ), // 0x0024 +qmi_name_item(QMIDMS_GET_MSISDN_RESP), // 0x0024 +qmi_name_item(QMIDMS_GET_DEVICE_SERIAL_NUMBERS_REQ), // 0x0025 +qmi_name_item(QMIDMS_GET_DEVICE_SERIAL_NUMBERS_RESP), // 0x0025 +qmi_name_item(QMIDMS_UIM_SET_PIN_PROTECTION_REQ), // 0x0027 +qmi_name_item(QMIDMS_UIM_SET_PIN_PROTECTION_RESP), // 0x0027 +qmi_name_item(QMIDMS_UIM_VERIFY_PIN_REQ), // 0x0028 +qmi_name_item(QMIDMS_UIM_VERIFY_PIN_RESP), // 0x0028 +qmi_name_item(QMIDMS_UIM_UNBLOCK_PIN_REQ), // 0x0029 +qmi_name_item(QMIDMS_UIM_UNBLOCK_PIN_RESP), // 0x0029 +qmi_name_item(QMIDMS_UIM_CHANGE_PIN_REQ), // 0x002A +qmi_name_item(QMIDMS_UIM_CHANGE_PIN_RESP), // 0x002A +qmi_name_item(QMIDMS_UIM_GET_PIN_STATUS_REQ), // 0x002B +qmi_name_item(QMIDMS_UIM_GET_PIN_STATUS_RESP), // 0x002B +qmi_name_item(QMIDMS_GET_DEVICE_HARDWARE_REV_REQ), // 0x002C +qmi_name_item(QMIDMS_GET_DEVICE_HARDWARE_REV_RESP), // 0x002C +qmi_name_item(QMIDMS_GET_OPERATING_MODE_REQ), // 0x002D +qmi_name_item(QMIDMS_GET_OPERATING_MODE_RESP), // 0x002D +qmi_name_item(QMIDMS_SET_OPERATING_MODE_REQ), // 0x002E +qmi_name_item(QMIDMS_SET_OPERATING_MODE_RESP), // 0x002E +qmi_name_item(QMIDMS_GET_ACTIVATED_STATUS_REQ), // 0x0031 +qmi_name_item(QMIDMS_GET_ACTIVATED_STATUS_RESP), // 0x0031 +qmi_name_item(QMIDMS_ACTIVATE_AUTOMATIC_REQ), // 0x0032 +qmi_name_item(QMIDMS_ACTIVATE_AUTOMATIC_RESP), // 0x0032 +qmi_name_item(QMIDMS_ACTIVATE_MANUAL_REQ), // 0x0033 +qmi_name_item(QMIDMS_ACTIVATE_MANUAL_RESP), // 0x0033 +qmi_name_item(QMIDMS_UIM_GET_ICCID_REQ), // 0x003C +qmi_name_item(QMIDMS_UIM_GET_ICCID_RESP), // 0x003C +qmi_name_item(QMIDMS_UIM_GET_CK_STATUS_REQ), // 0x0040 +qmi_name_item(QMIDMS_UIM_GET_CK_STATUS_RESP), // 0x0040 +qmi_name_item(QMIDMS_UIM_SET_CK_PROTECTION_REQ), // 0x0041 +qmi_name_item(QMIDMS_UIM_SET_CK_PROTECTION_RESP), // 0x0041 +qmi_name_item(QMIDMS_UIM_UNBLOCK_CK_REQ), // 0x0042 +qmi_name_item(QMIDMS_UIM_UNBLOCK_CK_RESP), // 0x0042 +qmi_name_item(QMIDMS_UIM_GET_IMSI_REQ), // 0x0043 +qmi_name_item(QMIDMS_UIM_GET_IMSI_RESP), // 0x0043 +qmi_name_item(QMIDMS_UIM_GET_STATE_REQ), // 0x0044 +qmi_name_item(QMIDMS_UIM_GET_STATE_RESP), // 0x0044 +qmi_name_item(QMIDMS_GET_BAND_CAP_REQ), // 0x0045 +qmi_name_item(QMIDMS_GET_BAND_CAP_RESP), // 0x0045 +}; + +static const QMI_NAME_T qmux_nas_Type[] = { +// ======================= NAS ============================== +qmi_name_item(QMINAS_SET_EVENT_REPORT_REQ), // 0x0002 +qmi_name_item(QMINAS_SET_EVENT_REPORT_RESP), // 0x0002 +qmi_name_item(QMINAS_EVENT_REPORT_IND), // 0x0002 +qmi_name_item(QMINAS_GET_SIGNAL_STRENGTH_REQ), // 0x0020 +qmi_name_item(QMINAS_GET_SIGNAL_STRENGTH_RESP), // 0x0020 +qmi_name_item(QMINAS_PERFORM_NETWORK_SCAN_REQ), // 0x0021 +qmi_name_item(QMINAS_PERFORM_NETWORK_SCAN_RESP), // 0x0021 +qmi_name_item(QMINAS_INITIATE_NW_REGISTER_REQ), // 0x0022 +qmi_name_item(QMINAS_INITIATE_NW_REGISTER_RESP), // 0x0022 +qmi_name_item(QMINAS_INITIATE_ATTACH_REQ), // 0x0023 +qmi_name_item(QMINAS_INITIATE_ATTACH_RESP), // 0x0023 +qmi_name_item(QMINAS_GET_SERVING_SYSTEM_REQ), // 0x0024 +qmi_name_item(QMINAS_GET_SERVING_SYSTEM_RESP), // 0x0024 +qmi_name_item(QMINAS_SERVING_SYSTEM_IND), // 0x0024 +qmi_name_item(QMINAS_GET_HOME_NETWORK_REQ), // 0x0025 +qmi_name_item(QMINAS_GET_HOME_NETWORK_RESP), // 0x0025 +qmi_name_item(QMINAS_GET_PREFERRED_NETWORK_REQ), // 0x0026 +qmi_name_item(QMINAS_GET_PREFERRED_NETWORK_RESP), // 0x0026 +qmi_name_item(QMINAS_SET_PREFERRED_NETWORK_REQ), // 0x0027 +qmi_name_item(QMINAS_SET_PREFERRED_NETWORK_RESP), // 0x0027 +qmi_name_item(QMINAS_GET_FORBIDDEN_NETWORK_REQ), // 0x0028 +qmi_name_item(QMINAS_GET_FORBIDDEN_NETWORK_RESP), // 0x0028 +qmi_name_item(QMINAS_SET_FORBIDDEN_NETWORK_REQ), // 0x0029 +qmi_name_item(QMINAS_SET_FORBIDDEN_NETWORK_RESP), // 0x0029 +qmi_name_item(QMINAS_SET_TECHNOLOGY_PREF_REQ), // 0x002A +qmi_name_item(QMINAS_SET_TECHNOLOGY_PREF_RESP), // 0x002A +qmi_name_item(QMINAS_GET_RF_BAND_INFO_REQ), // 0x0031 +qmi_name_item(QMINAS_GET_RF_BAND_INFO_RESP), // 0x0031 +qmi_name_item(QMINAS_GET_PLMN_NAME_REQ), // 0x0044 +qmi_name_item(QMINAS_GET_PLMN_NAME_RESP), // 0x0044 +qmi_name_item(QUECTEL_PACKET_TRANSFER_START_IND), // 0X100 +qmi_name_item(QUECTEL_PACKET_TRANSFER_END_IND), // 0X101 +qmi_name_item(QMINAS_GET_SYS_INFO_REQ), // 0x004D +qmi_name_item(QMINAS_GET_SYS_INFO_RESP), // 0x004D +qmi_name_item(QMINAS_SYS_INFO_IND), // 0x004D +}; + +static const QMI_NAME_T qmux_wms_Type[] = { +// ======================= WMS ============================== +qmi_name_item(QMIWMS_SET_EVENT_REPORT_REQ), // 0x0001 +qmi_name_item(QMIWMS_SET_EVENT_REPORT_RESP), // 0x0001 +qmi_name_item(QMIWMS_EVENT_REPORT_IND), // 0x0001 +qmi_name_item(QMIWMS_RAW_SEND_REQ), // 0x0020 +qmi_name_item(QMIWMS_RAW_SEND_RESP), // 0x0020 +qmi_name_item(QMIWMS_RAW_WRITE_REQ), // 0x0021 +qmi_name_item(QMIWMS_RAW_WRITE_RESP), // 0x0021 +qmi_name_item(QMIWMS_RAW_READ_REQ), // 0x0022 +qmi_name_item(QMIWMS_RAW_READ_RESP), // 0x0022 +qmi_name_item(QMIWMS_MODIFY_TAG_REQ), // 0x0023 +qmi_name_item(QMIWMS_MODIFY_TAG_RESP), // 0x0023 +qmi_name_item(QMIWMS_DELETE_REQ), // 0x0024 +qmi_name_item(QMIWMS_DELETE_RESP), // 0x0024 +qmi_name_item(QMIWMS_GET_MESSAGE_PROTOCOL_REQ), // 0x0030 +qmi_name_item(QMIWMS_GET_MESSAGE_PROTOCOL_RESP), // 0x0030 +qmi_name_item(QMIWMS_LIST_MESSAGES_REQ), // 0x0031 +qmi_name_item(QMIWMS_LIST_MESSAGES_RESP), // 0x0031 +qmi_name_item(QMIWMS_GET_SMSC_ADDRESS_REQ), // 0x0034 +qmi_name_item(QMIWMS_GET_SMSC_ADDRESS_RESP), // 0x0034 +qmi_name_item(QMIWMS_SET_SMSC_ADDRESS_REQ), // 0x0035 +qmi_name_item(QMIWMS_SET_SMSC_ADDRESS_RESP), // 0x0035 +qmi_name_item(QMIWMS_GET_STORE_MAX_SIZE_REQ), // 0x0036 +qmi_name_item(QMIWMS_GET_STORE_MAX_SIZE_RESP), // 0x0036 +}; + +static const QMI_NAME_T qmux_wds_admin_Type[] = { +qmi_name_item(QMIWDS_ADMIN_SET_DATA_FORMAT_REQ), // 0x0020 +qmi_name_item(QMIWDS_ADMIN_SET_DATA_FORMAT_RESP), // 0x0020 +qmi_name_item(QMIWDS_ADMIN_GET_DATA_FORMAT_REQ), // 0x0021 +qmi_name_item(QMIWDS_ADMIN_GET_DATA_FORMAT_RESP), // 0x0021 +qmi_name_item(QMIWDS_ADMIN_SET_QMAP_SETTINGS_REQ), // 0x002B +qmi_name_item(QMIWDS_ADMIN_SET_QMAP_SETTINGS_RESP), // 0x002B +qmi_name_item(QMIWDS_ADMIN_GET_QMAP_SETTINGS_REQ), // 0x002C +qmi_name_item(QMIWDS_ADMIN_GET_QMAP_SETTINGS_RESP), // 0x002C +}; + +static const QMI_NAME_T qmux_uim_Type[] = { +qmi_name_item( QMIUIM_READ_TRANSPARENT_REQ), // 0x0020 +qmi_name_item( QMIUIM_READ_TRANSPARENT_RESP), // 0x0020 +qmi_name_item( QMIUIM_READ_TRANSPARENT_IND), // 0x0020 +qmi_name_item( QMIUIM_READ_RECORD_REQ), // 0x0021 +qmi_name_item( QMIUIM_READ_RECORD_RESP), // 0x0021 +qmi_name_item( QMIUIM_READ_RECORD_IND), // 0x0021 +qmi_name_item( QMIUIM_WRITE_TRANSPARENT_REQ), // 0x0022 +qmi_name_item( QMIUIM_WRITE_TRANSPARENT_RESP), // 0x0022 +qmi_name_item( QMIUIM_WRITE_TRANSPARENT_IND), // 0x0022 +qmi_name_item( QMIUIM_WRITE_RECORD_REQ), // 0x0023 +qmi_name_item( QMIUIM_WRITE_RECORD_RESP), // 0x0023 +qmi_name_item( QMIUIM_WRITE_RECORD_IND), // 0x0023 +qmi_name_item( QMIUIM_SET_PIN_PROTECTION_REQ), // 0x0025 +qmi_name_item( QMIUIM_SET_PIN_PROTECTION_RESP), // 0x0025 +qmi_name_item( QMIUIM_SET_PIN_PROTECTION_IND), // 0x0025 +qmi_name_item( QMIUIM_VERIFY_PIN_REQ), // 0x0026 +qmi_name_item( QMIUIM_VERIFY_PIN_RESP), // 0x0026 +qmi_name_item( QMIUIM_VERIFY_PIN_IND), // 0x0026 +qmi_name_item( QMIUIM_UNBLOCK_PIN_REQ), // 0x0027 +qmi_name_item( QMIUIM_UNBLOCK_PIN_RESP), // 0x0027 +qmi_name_item( QMIUIM_UNBLOCK_PIN_IND), // 0x0027 +qmi_name_item( QMIUIM_CHANGE_PIN_REQ), // 0x0028 +qmi_name_item( QMIUIM_CHANGE_PIN_RESP), // 0x0028 +qmi_name_item( QMIUIM_CHANGE_PIN_IND), // 0x0028 +qmi_name_item( QMIUIM_DEPERSONALIZATION_REQ), // 0x0029 +qmi_name_item( QMIUIM_DEPERSONALIZATION_RESP), // 0x0029 +qmi_name_item( QMIUIM_EVENT_REG_REQ), // 0x002E +qmi_name_item( QMIUIM_EVENT_REG_RESP), // 0x002E +qmi_name_item( QMIUIM_GET_CARD_STATUS_REQ), // 0x002F +qmi_name_item( QMIUIM_GET_CARD_STATUS_RESP), // 0x002F +qmi_name_item( QMIUIM_STATUS_CHANGE_IND), // 0x0032 +}; + +static const char * qmi_name_get(const QMI_NAME_T *table, size_t size, int type, const char *tag) { + static char unknow[40]; + size_t i; + + if (qmux_CtlFlags == table) { + if (!strcmp(tag, "_REQ")) + tag = "_CMD"; + else if (!strcmp(tag, "_RESP")) + tag = "_RSP"; + } + + for (i = 0; i < size; i++) { + if (table[i].type == (UINT)type) { + if (!tag || (strstr(table[i].name, tag))) + return table[i].name; + } + } + sprintf(unknow, "unknow_%x", type); + return unknow; +} + +#define QMI_NAME(table, type) qmi_name_get(table, sizeof(table) / sizeof(table[0]), type, 0) +#define QMUX_NAME(table, type, tag) qmi_name_get(table, sizeof(table) / sizeof(table[0]), type, tag) + +void dump_tlv(PQCQMUX_MSG_HDR pQMUXMsgHdr) { + int TLVFind = 0; + int i; + //dbg("QCQMUX_TLV-----------------------------------\n"); + //dbg("{Type,\tLength,\tValue}\n"); + + while (1) { + PQMI_TLV_HDR TLVHdr = GetTLV(pQMUXMsgHdr, 0x1000 + (++TLVFind)); + if (TLVHdr == NULL) + break; + + //if ((TLVHdr->TLVType == 0x02) && ((USHORT *)(TLVHdr+1))[0]) + { + dbg("{%02x,\t%04x,\t", TLVHdr->TLVType, le16_to_cpu(TLVHdr->TLVLength)); + for (i = 0; i < le16_to_cpu(TLVHdr->TLVLength); i++) { + dbg("%02x ", ((UCHAR *)(TLVHdr+1))[i]); + } + dbg("}\n"); + } + } // while +} + +void dump_ctl(PQCQMICTL_MSG_HDR CTLHdr) { + const char *tag; + + //dbg("QCQMICTL_MSG--------------------------------------------\n"); + //dbg("CtlFlags: %02x\t\t%s\n", CTLHdr->CtlFlags, QMI_NAME(qmi_ctl_CtlFlags, CTLHdr->CtlFlags)); + dbg("TransactionId: %02x\n", CTLHdr->TransactionId); + switch (CTLHdr->CtlFlags) { + case QMICTL_FLAG_REQUEST: tag = "_REQ"; break; + case QMICTL_FLAG_RESPONSE: tag = "_RESP"; break; + case QMICTL_FLAG_INDICATION: tag = "_IND"; break; + default: tag = 0; break; + } + dbg("QMICTLType: %04x\t%s\n", le16_to_cpu(CTLHdr->QMICTLType), + QMUX_NAME(qmux_ctl_QMICTLType, le16_to_cpu(CTLHdr->QMICTLType), tag)); + dbg("Length: %04x\n", le16_to_cpu(CTLHdr->Length)); + + dump_tlv((PQCQMUX_MSG_HDR)(&CTLHdr->QMICTLType)); +} + +int dump_qmux(QMI_SERVICE_TYPE serviceType, PQCQMUX_HDR QMUXHdr) { + PQCQMUX_MSG_HDR QMUXMsgHdr = (PQCQMUX_MSG_HDR) (QMUXHdr + 1); + CHAR *tag; + + //dbg("QCQMUX--------------------------------------------\n"); + switch (QMUXHdr->CtlFlags&QMUX_CTL_FLAG_MASK_TYPE) { + case QMUX_CTL_FLAG_TYPE_CMD: tag = "_REQ"; break; + case QMUX_CTL_FLAG_TYPE_RSP: tag = "_RESP"; break; + case QMUX_CTL_FLAG_TYPE_IND: tag = "_IND"; break; + default: tag = 0; break; + } + //dbg("CtlFlags: %02x\t\t%s\n", QMUXHdr->CtlFlags, QMUX_NAME(qmux_CtlFlags, QMUXHdr->CtlFlags, tag)); + dbg("TransactionId: %04x\n", le16_to_cpu(QMUXHdr->TransactionId)); + + //dbg("QCQMUX_MSG_HDR-----------------------------------\n"); + switch (serviceType) { + case QMUX_TYPE_DMS: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), + QMUX_NAME(qmux_dms_Type, le16_to_cpu(QMUXMsgHdr->Type), tag)); + break; + case QMUX_TYPE_NAS: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), + QMUX_NAME(qmux_nas_Type, le16_to_cpu(QMUXMsgHdr->Type), tag)); + break; + case QMUX_TYPE_WDS: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), + QMUX_NAME(qmux_wds_Type, le16_to_cpu(QMUXMsgHdr->Type), tag)); + break; + case QMUX_TYPE_WMS: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), + QMUX_NAME(qmux_wms_Type, le16_to_cpu(QMUXMsgHdr->Type), tag)); + break; + case QMUX_TYPE_WDS_ADMIN: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), + QMUX_NAME(qmux_wds_admin_Type, le16_to_cpu(QMUXMsgHdr->Type), tag)); + break; + case QMUX_TYPE_UIM: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), + QMUX_NAME(qmux_uim_Type, le16_to_cpu(QMUXMsgHdr->Type), tag)); + break; + case QMUX_TYPE_PDS: + case QMUX_TYPE_QOS: + case QMUX_TYPE_CTL: + default: + dbg("Type: %04x\t%s\n", le16_to_cpu(QMUXMsgHdr->Type), "PDS/QOS/CTL/unknown!"); + break; + } + dbg("Length: %04x\n", le16_to_cpu(QMUXMsgHdr->Length)); + + dump_tlv(QMUXMsgHdr); + + return 0; +} + +void dump_qmi(void *dataBuffer, int dataLen) +{ + PQCQMI_HDR QMIHdr = (PQCQMI_HDR)dataBuffer; + PQCQMUX_HDR QMUXHdr = (PQCQMUX_HDR) (QMIHdr + 1); + PQCQMICTL_MSG_HDR CTLHdr = (PQCQMICTL_MSG_HDR) (QMIHdr + 1); + + int i; + + if (!debug_qmi) + return; + + pthread_mutex_lock(&dumpQMIMutex); + line[0] = 0; + for (i = 0; i < dataLen; i++) { + dbg("%02x ", ((unsigned char *)dataBuffer)[i]); + } + dbg_time("%s", line); + line[0] = 0; + + //dbg("QCQMI_HDR-----------------------------------------"); + //dbg("IFType: %02x\t\t%s", QMIHdr->IFType, QMI_NAME(qmi_IFType, QMIHdr->IFType)); + //dbg("Length: %04x", le16_to_cpu(QMIHdr->Length)); + //dbg("CtlFlags: %02x\t\t%s", QMIHdr->CtlFlags, QMI_NAME(qmi_CtlFlags, QMIHdr->CtlFlags)); + //dbg("QMIType: %02x\t\t%s", QMIHdr->QMIType, QMI_NAME(qmi_QMIType, QMIHdr->QMIType)); + //dbg("ClientId: %02x", QMIHdr->ClientId); + + if (QMIHdr->QMIType == QMUX_TYPE_CTL) { + dump_ctl(CTLHdr); + } else { + dump_qmux(QMIHdr->QMIType, QMUXHdr); + } + dbg_time("%s", line); + pthread_mutex_unlock(&dumpQMIMutex); +} diff --git a/quectel-CM/MPQMUX.h b/quectel-CM/MPQMUX.h new file mode 100755 index 0000000..c07fa69 --- /dev/null +++ b/quectel-CM/MPQMUX.h @@ -0,0 +1,3321 @@ +/*=========================================================================== + + M P Q M U X. H +DESCRIPTION: + + This file provides support for QMUX. + +INITIALIZATION AND SEQUENCING REQUIREMENTS: + +Copyright (C) 2011 by Qualcomm Technologies, Incorporated. All Rights Reserved. +===========================================================================*/ + +#ifndef MPQMUX_H +#define MPQMUX_H + +#include "MPQMI.h" + +#pragma pack(push, 1) + +#define QMIWDS_SET_EVENT_REPORT_REQ 0x0001 +#define QMIWDS_SET_EVENT_REPORT_RESP 0x0001 +#define QMIWDS_EVENT_REPORT_IND 0x0001 +#define QMIWDS_START_NETWORK_INTERFACE_REQ 0x0020 +#define QMIWDS_START_NETWORK_INTERFACE_RESP 0x0020 +#define QMIWDS_STOP_NETWORK_INTERFACE_REQ 0x0021 +#define QMIWDS_STOP_NETWORK_INTERFACE_RESP 0x0021 +#define QMIWDS_GET_PKT_SRVC_STATUS_REQ 0x0022 +#define QMIWDS_GET_PKT_SRVC_STATUS_RESP 0x0022 +#define QMIWDS_GET_PKT_SRVC_STATUS_IND 0x0022 +#define QMIWDS_GET_CURRENT_CHANNEL_RATE_REQ 0x0023 +#define QMIWDS_GET_CURRENT_CHANNEL_RATE_RESP 0x0023 +#define QMIWDS_GET_PKT_STATISTICS_REQ 0x0024 +#define QMIWDS_GET_PKT_STATISTICS_RESP 0x0024 +#define QMIWDS_MODIFY_PROFILE_SETTINGS_REQ 0x0028 +#define QMIWDS_MODIFY_PROFILE_SETTINGS_RESP 0x0028 +#define QMIWDS_GET_PROFILE_SETTINGS_REQ 0x002B +#define QMIWDS_GET_PROFILE_SETTINGS_RESP 0x002B +#define QMIWDS_GET_DEFAULT_SETTINGS_REQ 0x002C +#define QMIWDS_GET_DEFAULT_SETTINGS_RESP 0x002C +#define QMIWDS_GET_RUNTIME_SETTINGS_REQ 0x002D +#define QMIWDS_GET_RUNTIME_SETTINGS_RESP 0x002D +#define QMIWDS_GET_MIP_MODE_REQ 0x002F +#define QMIWDS_GET_MIP_MODE_RESP 0x002F +#define QMIWDS_GET_DATA_BEARER_REQ 0x0037 +#define QMIWDS_GET_DATA_BEARER_RESP 0x0037 +#define QMIWDS_DUN_CALL_INFO_REQ 0x0038 +#define QMIWDS_DUN_CALL_INFO_RESP 0x0038 +#define QMIWDS_DUN_CALL_INFO_IND 0x0038 +#define QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ 0x004D +#define QMIWDS_SET_CLIENT_IP_FAMILY_PREF_RESP 0x004D +#define QMIWDS_SET_AUTO_CONNECT_REQ 0x0051 +#define QMIWDS_SET_AUTO_CONNECT_RESP 0x0051 +#define QMIWDS_BIND_MUX_DATA_PORT_REQ 0x00A2 +#define QMIWDS_BIND_MUX_DATA_PORT_RESP 0x00A2 + + +// Stats masks +#define QWDS_STAT_MASK_TX_PKT_OK 0x00000001 +#define QWDS_STAT_MASK_RX_PKT_OK 0x00000002 +#define QWDS_STAT_MASK_TX_PKT_ER 0x00000004 +#define QWDS_STAT_MASK_RX_PKT_ER 0x00000008 +#define QWDS_STAT_MASK_TX_PKT_OF 0x00000010 +#define QWDS_STAT_MASK_RX_PKT_OF 0x00000020 + +// TLV Types for xfer statistics +#define TLV_WDS_TX_GOOD_PKTS 0x10 +#define TLV_WDS_RX_GOOD_PKTS 0x11 +#define TLV_WDS_TX_ERROR 0x12 +#define TLV_WDS_RX_ERROR 0x13 +#define TLV_WDS_TX_OVERFLOW 0x14 +#define TLV_WDS_RX_OVERFLOW 0x15 +#define TLV_WDS_CHANNEL_RATE 0x16 +#define TLV_WDS_DATA_BEARER 0x17 +#define TLV_WDS_DORMANCY_STATUS 0x18 + +#define QWDS_PKT_DATA_DISCONNECTED 0x01 +#define QWDS_PKT_DATA_CONNECTED 0x02 +#define QWDS_PKT_DATA_SUSPENDED 0x03 +#define QWDS_PKT_DATA_AUTHENTICATING 0x04 + +#define QMIWDS_ADMIN_SET_DATA_FORMAT_REQ 0x0020 +#define QMIWDS_ADMIN_SET_DATA_FORMAT_RESP 0x0020 +#define QMIWDS_ADMIN_GET_DATA_FORMAT_REQ 0x0021 +#define QMIWDS_ADMIN_GET_DATA_FORMAT_RESP 0x0021 +#define QMIWDS_ADMIN_SET_QMAP_SETTINGS_REQ 0x002B +#define QMIWDS_ADMIN_SET_QMAP_SETTINGS_RESP 0x002B +#define QMIWDS_ADMIN_GET_QMAP_SETTINGS_REQ 0x002C +#define QMIWDS_ADMIN_GET_QMAP_SETTINGS_RESP 0x002C + +#define NETWORK_DESC_ENCODING_OCTET 0x00 +#define NETWORK_DESC_ENCODING_EXTPROTOCOL 0x01 +#define NETWORK_DESC_ENCODING_7BITASCII 0x02 +#define NETWORK_DESC_ENCODING_IA5 0x03 +#define NETWORK_DESC_ENCODING_UNICODE 0x04 +#define NETWORK_DESC_ENCODING_SHIFTJIS 0x05 +#define NETWORK_DESC_ENCODING_KOREAN 0x06 +#define NETWORK_DESC_ENCODING_LATINH 0x07 +#define NETWORK_DESC_ENCODING_LATIN 0x08 +#define NETWORK_DESC_ENCODING_GSM7BIT 0x09 +#define NETWORK_DESC_ENCODING_GSMDATA 0x0A +#define NETWORK_DESC_ENCODING_UNKNOWN 0xFF + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT +{ + USHORT Type; // QMUX type 0x0000 + USHORT Length; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT, *PQMIWDS_ADMIN_SET_DATA_FORMAT; + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR QOSSetting; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS, *PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS; + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + ULONG Value; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_TLV, *PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV; + +typedef struct _QMIWDS_ENDPOINT_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + ULONG ep_type; + ULONG iface_id; +} __attribute__ ((packed)) QMIWDS_ENDPOINT_TLV, *PQMIWDS_ENDPOINT_TLV; + + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG +{ + USHORT Type; + USHORT Length; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS QosDataFormatTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UnderlyingLinkLayerProtocolTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationProtocolTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationProtocolTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationMaxDatagramsTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationMaxSizeTlv; +#if 0 + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationMaxDatagramsTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationMaxSizeTlv; +#else + QMIWDS_ENDPOINT_TLV epTlv; +#endif +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG, *PQMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG; + +#if 0 +typedef enum _QMI_RETURN_CODES { + QMI_SUCCESS = 0, + QMI_SUCCESS_NOT_COMPLETE, + QMI_FAILURE +}QMI_RETURN_CODES; + +typedef struct _QMIWDS_GET_PKT_SRVC_STATUS_REQ_MSG +{ + USHORT Type; // 0x0022 + USHORT Length; // 0x0000 +} QMIWDS_GET_PKT_SRVC_STATUS_REQ_MSG, *PQMIWDS_GET_PKT_SRVC_STATUS_REQ_MSG; + +typedef struct _QMIWDS_GET_PKT_SRVC_STATUS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLVType2; + USHORT TLVLength2; + UCHAR ConnectionStatus; // 0x01: QWDS_PKT_DATAC_DISCONNECTED + // 0x02: QWDS_PKT_DATA_CONNECTED + // 0x03: QWDS_PKT_DATA_SUSPENDED + // 0x04: QWDS_PKT_DATA_AUTHENTICATING +} QMIWDS_GET_PKT_SRVC_STATUS_RESP_MSG, *PQMIWDS_GET_PKT_SRVC_STATUS_RESP_MSG; + +typedef struct _QMIWDS_GET_PKT_SRVC_STATUS_IND_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR ConnectionStatus; // 0x01: QWDS_PKT_DATAC_DISCONNECTED + // 0x02: QWDS_PKT_DATA_CONNECTED + // 0x03: QWDS_PKT_DATA_SUSPENDED + UCHAR ReconfigRequired; // 0x00: No need to reconfigure + // 0x01: Reconfiguration required +} QMIWDS_GET_PKT_SRVC_STATUS_IND_MSG, *PQMIWDS_GET_PKT_SRVC_STATUS_IND_MSG; + +typedef struct _WDS_PKT_SRVC_IP_FAMILY_TLV +{ + UCHAR TLVType; // 0x12 + USHORT TLVLength; // 1 + UCHAR IpFamily; // IPV4-0x04, IPV6-0x06 +} WDS_PKT_SRVC_IP_FAMILY_TLV, *PWDS_PKT_SRVC_IP_FAMILY_TLV; + +typedef struct _QMIWDS_DUN_CALL_INFO_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + ULONG Mask; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR ReportConnectionStatus; +} QMIWDS_DUN_CALL_INFO_REQ_MSG, *PQMIWDS_DUN_CALL_INFO_REQ_MSG; + +typedef struct _QMIWDS_DUN_CALL_INFO_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMIWDS_DUN_CALL_INFO_RESP_MSG, *PQMIWDS_DUN_CALL_INFO_RESP_MSG; + +typedef struct _QMIWDS_DUN_CALL_INFO_IND_MSG +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ConnectionStatus; +} QMIWDS_DUN_CALL_INFO_IND_MSG, *PQMIWDS_DUN_CALL_INFO_IND_MSG; + +typedef struct _QMIWDS_GET_CURRENT_CHANNEL_RATE_REQ_MSG +{ + USHORT Type; // QMUX type 0x0040 + USHORT Length; +} QMIWDS_GET_CURRENT_CHANNEL_RATE_REQ_MSG, *PQMIWDS_GET_CURRENT_CHANNEL_RATE_REQ_MSG; + +typedef struct _QMIWDS_GET_CURRENT_CHANNEL_RATE_RESP_MSG +{ + USHORT Type; // QMUX type 0x0040 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 16 + //ULONG CallHandle; // Context corresponding to reported channel + ULONG CurrentTxRate; // bps + ULONG CurrentRxRate; // bps + ULONG ServingSystemTxRate; // bps + ULONG ServingSystemRxRate; // bps + +} QMIWDS_GET_CURRENT_CHANNEL_RATE_RESP_MSG, *PQMIWDS_GET_CURRENT_CHANNEL_RATE_RESP; + +#define QWDS_EVENT_REPORT_MASK_RATES 0x01 +#define QWDS_EVENT_REPORT_MASK_STATS 0x02 + +#ifdef QCUSB_MUX_PROTOCOL +#error code not present +#endif // QCUSB_MUX_PROTOCOL + +typedef struct _QMIWDS_SET_EVENT_REPORT_REQ_MSG +{ + USHORT Type; // QMUX type 0x0042 + USHORT Length; + + UCHAR TLVType; // 0x10 -- current channel rate indicator + USHORT TLVLength; // 1 + UCHAR Mode; // 0-do not report; 1-report when rate changes + + UCHAR TLV2Type; // 0x11 + USHORT TLV2Length; // 5 + UCHAR StatsPeriod; // seconds between reports; 0-do not report + ULONG StatsMask; // + + UCHAR TLV3Type; // 0x12 -- current data bearer indicator + USHORT TLV3Length; // 1 + UCHAR Mode3; // 0-do not report; 1-report when changes + + UCHAR TLV4Type; // 0x13 -- dormancy status indicator + USHORT TLV4Length; // 1 + UCHAR DormancyStatus; // 0-do not report; 1-report when changes +} QMIWDS_SET_EVENT_REPORT_REQ_MSG, *PQMIWDS_SET_EVENT_REPORT_REQ_MSG; + +typedef struct _QMIWDS_SET_EVENT_REPORT_RESP_MSG +{ + USHORT Type; // QMUX type 0x0042 + USHORT Length; + + UCHAR TLVType; // 0x02 result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_NO_BATTERY + // QMI_ERR_FAULT +} QMIWDS_SET_EVENT_REPORT_RESP_MSG, *PQMIWDS_SET_EVENT_REPORT_RESP_MSG; + +typedef struct _QMIWDS_EVENT_REPORT_IND_MSG +{ + USHORT Type; // QMUX type 0x0001 + USHORT Length; +} QMIWDS_EVENT_REPORT_IND_MSG, *PQMIWDS_EVENT_REPORT_IND_MSG; + +// PQCTLV_PKT_STATISTICS + +typedef struct _QMIWDS_EVENT_REPORT_IND_CHAN_RATE_TLV +{ + UCHAR Type; + USHORT Length; // 8 + ULONG TxRate; + ULONG RxRate; +} QMIWDS_EVENT_REPORT_IND_CHAN_RATE_TLV, *PQMIWDS_EVENT_REPORT_IND_CHAN_RATE_TLV; + +#ifdef QCUSB_MUX_PROTOCOL +#error code not present +#endif // QCUSB_MUX_PROTOCOL + +typedef struct _QMIWDS_GET_PKT_STATISTICS_REQ_MSG +{ + USHORT Type; // QMUX type 0x0041 + USHORT Length; + UCHAR TLVType; // 0x01 + USHORT TLVLength; // 4 + ULONG StateMask; // 0x00000001 tx success packets + // 0x00000002 rx success packets + // 0x00000004 rx packet errors (checksum) + // 0x00000008 rx packets dropped (memory) + +} QMIWDS_GET_PKT_STATISTICS_REQ_MSG, *PQMIWDS_GET_PKT_STATISTICS_REQ_MSG; + +typedef struct _QMIWDS_GET_PKT_STATISTICS_RESP_MSG +{ + USHORT Type; // QMUX type 0x0041 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMIWDS_GET_PKT_STATISTICS_RESP_MSG, *PQMIWDS_GET_PKT_STATISTICS_RESP_MSG; + +// optional TLV for stats +typedef struct _QCTLV_PKT_STATISTICS +{ + UCHAR TLVType; // see above definitions for TLV types + USHORT TLVLength; // 4 + ULONG Count; +} QCTLV_PKT_STATISTICS, *PQCTLV_PKT_STATISTICS; +#endif + +//#ifdef QC_IP_MODE + +#define QMIWDS_GET_RUNTIME_SETTINGS_MASK_IPV4DNS_ADDR 0x0010 +#define QMIWDS_GET_RUNTIME_SETTINGS_MASK_IPV4_ADDR 0x0100 +#define QMIWDS_GET_RUNTIME_SETTINGS_MASK_IPV4GATEWAY_ADDR 0x0200 +#define QMIWDS_GET_RUNTIME_SETTINGS_MASK_MTU 0x2000 + +typedef struct _QMIWDS_GET_RUNTIME_SETTINGS_REQ_MSG +{ + USHORT Type; // QMIWDS_GET_RUNTIME_SETTINGS_REQ + USHORT Length; + UCHAR TLVType; // 0x10 + USHORT TLVLength; // 0x0004 + ULONG Mask; // mask, bit 8: IP addr -- 0x0100 +} __attribute__ ((packed)) QMIWDS_GET_RUNTIME_SETTINGS_REQ_MSG, *PQMIWDS_GET_RUNTIME_SETTINGS_REQ_MSG; + +typedef struct _QMIWDS_BIND_MUX_DATA_PORT_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + ULONG ep_type; + ULONG iface_id; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR MuxId; + UCHAR TLV3Type; + USHORT TLV3Length; + ULONG client_type; +} __attribute__ ((packed)) QMIWDS_BIND_MUX_DATA_PORT_REQ_MSG, *PQMIWDS_BIND_MUX_DATA_PORT_REQ_MSG; + +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4PRIMARYDNS 0x15 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4SECONDARYDNS 0x16 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4 0x1E +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4GATEWAY 0x20 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4SUBNET 0x21 + +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6 0x25 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6GATEWAY 0x26 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6PRIMARYDNS 0x27 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6SECONDARYDNS 0x28 +#define QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_MTU 0x29 + +typedef struct _QMIWDS_GET_RUNTIME_SETTINGS_TLV_MTU +{ + UCHAR TLVType; // QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_MTU + USHORT TLVLength; // 4 + ULONG Mtu; // MTU +} __attribute__ ((packed)) QMIWDS_GET_RUNTIME_SETTINGS_TLV_MTU, *PQMIWDS_GET_RUNTIME_SETTINGS_TLV_MTU; + +typedef struct _QMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR +{ + UCHAR TLVType; // QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4 + USHORT TLVLength; // 4 + ULONG IPV4Address; // address +} __attribute__ ((packed)) QMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR, *PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR; + +typedef struct _QMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR +{ + UCHAR TLVType; // QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6 + USHORT TLVLength; // 16 + UCHAR IPV6Address[16]; // address + UCHAR PrefixLength; // prefix length +} __attribute__ ((packed)) QMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR, *PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR; + +typedef struct _QMIWDS_GET_RUNTIME_SETTINGS_RESP_MSG +{ + USHORT Type; // QMIWDS_GET_RUNTIME_SETTINGS_RESP + USHORT Length; + UCHAR TLVType; // QCTLV_TYPE_RESULT_CODE + USHORT TLVLength; // 0x0004 + USHORT QMUXResult; // result code + USHORT QMUXError; // error code +} __attribute__ ((packed)) QMIWDS_GET_RUNTIME_SETTINGS_RESP_MSG, *PQMIWDS_GET_RUNTIME_SETTINGS_RESP_MSG; + +//#endif // QC_IP_MODE + +typedef struct _QMIWDS_IP_FAMILY_TLV +{ + UCHAR TLVType; // 0x12 + USHORT TLVLength; // 1 + UCHAR IpFamily; // IPV4-0x04, IPV6-0x06 +} __attribute__ ((packed)) QMIWDS_IP_FAMILY_TLV, *PQMIWDS_IP_FAMILY_TLV; + +typedef struct _QMIWDS_PKT_SRVC_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ConnectionStatus; + UCHAR ReconfigReqd; +} __attribute__ ((packed)) QMIWDS_PKT_SRVC_TLV, *PQMIWDS_PKT_SRVC_TLV; + +typedef struct _QMIWDS_CALL_END_REASON_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT CallEndReason; +} __attribute__ ((packed)) QMIWDS_CALL_END_REASON_TLV, *PQMIWDS_CALL_END_REASON_TLV; + +typedef struct _QMIWDS_CALL_END_REASON_V_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT CallEndReasonType; + USHORT CallEndReason; +} __attribute__ ((packed)) QMIWDS_CALL_END_REASON_V_TLV, *PQMIWDS_CALL_END_REASON_V_TLV; + +typedef struct _QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ_MSG +{ + USHORT Type; // QMUX type 0x004D + USHORT Length; + UCHAR TLVType; // 0x01 + USHORT TLVLength; // 1 + UCHAR IpPreference; // IPV4-0x04, IPV6-0x06 +} __attribute__ ((packed)) QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ_MSG, *PQMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ_MSG; + +typedef struct _QMIWDS_SET_CLIENT_IP_FAMILY_PREF_RESP_MSG +{ + USHORT Type; // QMUX type 0x0037 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS, QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INTERNAL, QMI_ERR_MALFORMED_MSG, QMI_ERR_INVALID_ARG +} __attribute__ ((packed)) QMIWDS_SET_CLIENT_IP_FAMILY_PREF_RESP_MSG, *PQMIWDS_SET_CLIENT_IP_FAMILY_PREF_RESP_MSG; + +typedef struct _QMIWDS_SET_AUTO_CONNECT_REQ_MSG +{ + USHORT Type; // QMUX type 0x0051 + USHORT Length; + UCHAR TLVType; // 0x01 + USHORT TLVLength; // 1 + UCHAR autoconnect_setting; // 0x00 ?C Disabled, 0x01 ?C Enabled, 0x02 ?C Paused (resume on power cycle) +} __attribute__ ((packed)) QMIWDS_SET_AUTO_CONNECT_REQ_MSG, *PQMIWDS_SET_AUTO_CONNECT_REQ_MSG; + +#if 0 +typedef struct _QMIWDS_GET_MIP_MODE_REQ_MSG +{ + USHORT Type; // QMUX type 0x0040 + USHORT Length; +} QMIWDS_GET_MIP_MODE_REQ_MSG, *PQMIWDS_GET_MIP_MODE_REQ_MSG; + +typedef struct _QMIWDS_GET_MIP_MODE_RESP_MSG +{ + USHORT Type; // QMUX type 0x0040 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 20 + UCHAR MipMode; // +} QMIWDS_GET_MIP_MODE_RESP_MSG, *PQMIWDS_GET_MIP_MODE_RESP_MSG; +#endif + +typedef struct _QMIWDS_TECHNOLOGY_PREFERECE +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR TechPreference; +} __attribute__ ((packed)) QMIWDS_TECHNOLOGY_PREFERECE, *PQMIWDS_TECHNOLOGY_PREFERECE; + +typedef struct _QMIWDS_PROFILE_IDENTIFIER +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ProfileIndex; +} __attribute__ ((packed)) QMIWDS_PROFILE_IDENTIFIER, *PQMIWDS_PROFILE_IDENTIFIER; + +#if 0 +typedef struct _QMIWDS_IPADDRESS +{ + UCHAR TLVType; + USHORT TLVLength; + ULONG IPv4Address; +}QMIWDS_IPADDRESS, *PQMIWDS_IPADDRESS; + +/* +typedef struct _QMIWDS_UMTS_QOS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR TrafficClass; + ULONG MaxUplinkBitRate; + ULONG MaxDownlinkBitRate; + ULONG GuarUplinkBitRate; + ULONG GuarDownlinkBitRate; + UCHAR QOSDevOrder; + ULONG MAXSDUSize; + UCHAR SDUErrorRatio; + UCHAR ResidualBerRatio; + UCHAR DeliveryErrorSDUs; + ULONG TransferDelay; + ULONG TrafficHndPri; +}QMIWDS_UMTS_QOS, *PQMIWDS_UMTS_QOS; + +typedef struct _QMIWDS_GPRS_QOS +{ + UCHAR TLVType; + USHORT TLVLength; + ULONG PrecedenceClass; + ULONG DelayClass; + ULONG ReliabilityClass; + ULONG PeekThroClass; + ULONG MeanThroClass; +}QMIWDS_GPRS_QOS, *PQMIWDS_GPRS_QOS; +*/ +#endif + +typedef struct _QMIWDS_PROFILENAME +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ProfileName; +} __attribute__ ((packed)) QMIWDS_PROFILENAME, *PQMIWDS_PROFILENAME; + +typedef struct _QMIWDS_PDPTYPE +{ + UCHAR TLVType; + USHORT TLVLength; +// 0 ?C PDP-IP (IPv4) +// 1 ?C PDP-PPP +// 2 ?C PDP-IPv6 +// 3 ?C PDP-IPv4v6 + UCHAR PdpType; +} __attribute__ ((packed)) QMIWDS_PDPTYPE, *PQMIWDS_PDPTYPE; + +typedef struct _QMIWDS_USERNAME +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR UserName; +} __attribute__ ((packed)) QMIWDS_USERNAME, *PQMIWDS_USERNAME; + +typedef struct _QMIWDS_PASSWD +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR Passwd; +} __attribute__ ((packed)) QMIWDS_PASSWD, *PQMIWDS_PASSWD; + +typedef struct _QMIWDS_AUTH_PREFERENCE +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR AuthPreference; +} __attribute__ ((packed)) QMIWDS_AUTH_PREFERENCE, *PQMIWDS_AUTH_PREFERENCE; + +typedef struct _QMIWDS_APNNAME +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ApnName; +} __attribute__ ((packed)) QMIWDS_APNNAME, *PQMIWDS_APNNAME; + +typedef struct _QMIWDS_AUTOCONNECT +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR AutoConnect; +} __attribute__ ((packed)) QMIWDS_AUTOCONNECT, *PQMIWDS_AUTOCONNECT; + +typedef struct _QMIWDS_START_NETWORK_INTERFACE_REQ_MSG +{ + USHORT Type; + USHORT Length; +} __attribute__ ((packed)) QMIWDS_START_NETWORK_INTERFACE_REQ_MSG, *PQMIWDS_START_NETWORK_INTERFACE_REQ_MSG; + +typedef struct _QMIWDS_CALLENDREASON +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT Reason; +}__attribute__ ((packed)) QMIWDS_CALLENDREASON, *PQMIWDS_CALLENDREASON; + +typedef struct _QMIWDS_START_NETWORK_INTERFACE_RESP_MSG +{ + USHORT Type; // QMUX type 0x0040 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 20 + ULONG Handle; // +} __attribute__ ((packed)) QMIWDS_START_NETWORK_INTERFACE_RESP_MSG, *PQMIWDS_START_NETWORK_INTERFACE_RESP_MSG; + +typedef struct _QMIWDS_STOP_NETWORK_INTERFACE_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + ULONG Handle; +} __attribute__ ((packed)) QMIWDS_STOP_NETWORK_INTERFACE_REQ_MSG, *PQMIWDS_STOP_NETWORK_INTERFACE_REQ_MSG; + +typedef struct _QMIWDS_STOP_NETWORK_INTERFACE_RESP_MSG +{ + USHORT Type; // QMUX type 0x0040 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + +} __attribute__ ((packed)) QMIWDS_STOP_NETWORK_INTERFACE_RESP_MSG, *PQMIWDS_STOP_NETWORK_INTERFACE_RESP_MSG; + +typedef struct _QMIWDS_GET_DEFAULT_SETTINGS_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR ProfileType; +} __attribute__ ((packed)) QMIWDS_GET_DEFAULT_SETTINGS_REQ_MSG, *PQMIWDS_GET_DEFAULT_SETTINGS_REQ_MSG; + +typedef struct _QMIWDS_GET_DEFAULT_SETTINGS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} __attribute__ ((packed)) QMIWDS_GET_DEFAULT_SETTINGS_RESP_MSG, *PQMIWDS_GET_DEFAULT_SETTINGS_RESP_MSG; + +typedef struct _QMIWDS_MODIFY_PROFILE_SETTINGS_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR ProfileType; + UCHAR ProfileIndex; +} __attribute__ ((packed)) QMIWDS_MODIFY_PROFILE_SETTINGS_REQ_MSG, *PQMIWDS_MODIFY_PROFILE_SETTINGS_REQ_MSG; + +typedef struct _QMIWDS_MODIFY_PROFILE_SETTINGS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} __attribute__ ((packed)) QMIWDS_MODIFY_PROFILE_SETTINGS_RESP_MSG, *PQMIWDS_MODIFY_PROFILE_SETTINGS_RESP_MSG; + +typedef struct _QMIWDS_GET_PROFILE_SETTINGS_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR ProfileType; + UCHAR ProfileIndex; +} __attribute__ ((packed)) QMIWDS_GET_PROFILE_SETTINGS_REQ_MSG, *PQMIWDS_GET_PROFILE_SETTINGS_REQ_MSG; + +#if 0 +typedef struct _QMIWDS_EVENT_REPORT_IND_DATA_BEARER_TLV +{ + UCHAR Type; + USHORT Length; + UCHAR DataBearer; +} QMIWDS_EVENT_REPORT_IND_DATA_BEARER_TLV, *PQMIWDS_EVENT_REPORT_IND_DATA_BEARER_TLV; + +typedef struct _QMIWDS_EVENT_REPORT_IND_DORMANCY_STATUS_TLV +{ + UCHAR Type; + USHORT Length; + UCHAR DormancyStatus; +} QMIWDS_EVENT_REPORT_IND_DORMANCY_STATUS_TLV, *PQMIWDS_EVENT_REPORT_IND_DORMANCY_STATUS_TLV; + + +typedef struct _QMIWDS_GET_DATA_BEARER_REQ_MSG +{ + USHORT Type; // QMUX type 0x0037 + USHORT Length; +} QMIWDS_GET_DATA_BEARER_REQ_MSG, *PQMIWDS_GET_DATA_BEARER_REQ_MSG; + +typedef struct _QMIWDS_GET_DATA_BEARER_RESP_MSG +{ + USHORT Type; // QMUX type 0x0037 + USHORT Length; + UCHAR TLVType; // 0x02 + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INTERNAL + // QMI_ERR_MALFORMED_MSG + // QMI_ERR_NO_MEMORY + // QMI_ERR_OUT_OF_CALL + // QMI_ERR_INFO_UNAVAILABLE + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // + UCHAR Technology; // +} QMIWDS_GET_DATA_BEARER_RESP_MSG, *PQMIWDS_GET_DATA_BEARER_RESP_MSG; +#endif + +// ======================= DMS ============================== +#define QMIDMS_SET_EVENT_REPORT_REQ 0x0001 +#define QMIDMS_SET_EVENT_REPORT_RESP 0x0001 +#define QMIDMS_EVENT_REPORT_IND 0x0001 +#define QMIDMS_GET_DEVICE_CAP_REQ 0x0020 +#define QMIDMS_GET_DEVICE_CAP_RESP 0x0020 +#define QMIDMS_GET_DEVICE_MFR_REQ 0x0021 +#define QMIDMS_GET_DEVICE_MFR_RESP 0x0021 +#define QMIDMS_GET_DEVICE_MODEL_ID_REQ 0x0022 +#define QMIDMS_GET_DEVICE_MODEL_ID_RESP 0x0022 +#define QMIDMS_GET_DEVICE_REV_ID_REQ 0x0023 +#define QMIDMS_GET_DEVICE_REV_ID_RESP 0x0023 +#define QMIDMS_GET_MSISDN_REQ 0x0024 +#define QMIDMS_GET_MSISDN_RESP 0x0024 +#define QMIDMS_GET_DEVICE_SERIAL_NUMBERS_REQ 0x0025 +#define QMIDMS_GET_DEVICE_SERIAL_NUMBERS_RESP 0x0025 +#define QMIDMS_UIM_SET_PIN_PROTECTION_REQ 0x0027 +#define QMIDMS_UIM_SET_PIN_PROTECTION_RESP 0x0027 +#define QMIDMS_UIM_VERIFY_PIN_REQ 0x0028 +#define QMIDMS_UIM_VERIFY_PIN_RESP 0x0028 +#define QMIDMS_UIM_UNBLOCK_PIN_REQ 0x0029 +#define QMIDMS_UIM_UNBLOCK_PIN_RESP 0x0029 +#define QMIDMS_UIM_CHANGE_PIN_REQ 0x002A +#define QMIDMS_UIM_CHANGE_PIN_RESP 0x002A +#define QMIDMS_UIM_GET_PIN_STATUS_REQ 0x002B +#define QMIDMS_UIM_GET_PIN_STATUS_RESP 0x002B +#define QMIDMS_GET_DEVICE_HARDWARE_REV_REQ 0x002C +#define QMIDMS_GET_DEVICE_HARDWARE_REV_RESP 0x002C +#define QMIDMS_GET_OPERATING_MODE_REQ 0x002D +#define QMIDMS_GET_OPERATING_MODE_RESP 0x002D +#define QMIDMS_SET_OPERATING_MODE_REQ 0x002E +#define QMIDMS_SET_OPERATING_MODE_RESP 0x002E +#define QMIDMS_GET_ACTIVATED_STATUS_REQ 0x0031 +#define QMIDMS_GET_ACTIVATED_STATUS_RESP 0x0031 +#define QMIDMS_ACTIVATE_AUTOMATIC_REQ 0x0032 +#define QMIDMS_ACTIVATE_AUTOMATIC_RESP 0x0032 +#define QMIDMS_ACTIVATE_MANUAL_REQ 0x0033 +#define QMIDMS_ACTIVATE_MANUAL_RESP 0x0033 +#define QMIDMS_UIM_GET_ICCID_REQ 0x003C +#define QMIDMS_UIM_GET_ICCID_RESP 0x003C +#define QMIDMS_UIM_GET_CK_STATUS_REQ 0x0040 +#define QMIDMS_UIM_GET_CK_STATUS_RESP 0x0040 +#define QMIDMS_UIM_SET_CK_PROTECTION_REQ 0x0041 +#define QMIDMS_UIM_SET_CK_PROTECTION_RESP 0x0041 +#define QMIDMS_UIM_UNBLOCK_CK_REQ 0x0042 +#define QMIDMS_UIM_UNBLOCK_CK_RESP 0x0042 +#define QMIDMS_UIM_GET_IMSI_REQ 0x0043 +#define QMIDMS_UIM_GET_IMSI_RESP 0x0043 +#define QMIDMS_UIM_GET_STATE_REQ 0x0044 +#define QMIDMS_UIM_GET_STATE_RESP 0x0044 +#define QMIDMS_GET_BAND_CAP_REQ 0x0045 +#define QMIDMS_GET_BAND_CAP_RESP 0x0045 + +#if 0 +typedef struct _QMIDMS_GET_DEVICE_MFR_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMIDMS_GET_DEVICE_MFR_REQ_MSG, *PQMIDMS_GET_DEVICE_MFR_REQ_MSG; + +typedef struct _QMIDMS_GET_DEVICE_MFR_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; // 0x01 - required parameter + USHORT TLV2Length; // length of the mfr string + UCHAR DeviceManufacturer; // first byte of string +} QMIDMS_GET_DEVICE_MFR_RESP_MSG, *PQMIDMS_GET_DEVICE_MFR_RESP_MSG; + +typedef struct _QMIDMS_GET_DEVICE_MODEL_ID_REQ_MSG +{ + USHORT Type; // QMUX type 0x0004 + USHORT Length; +} QMIDMS_GET_DEVICE_MODEL_ID_REQ_MSG, *PQMIDMS_GET_DEVICE_MODEL_ID_REQ_MSG; + +typedef struct _QMIDMS_GET_DEVICE_MODEL_ID_RESP_MSG +{ + USHORT Type; // QMUX type 0x0004 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; // 0x01 - required parameter + USHORT TLV2Length; // length of the modem id string + UCHAR DeviceModelID; // device model id +} QMIDMS_GET_DEVICE_MODEL_ID_RESP_MSG, *PQMIDMS_GET_DEVICE_MODEL_ID_RESP_MSG; +#endif + +typedef struct _QMIDMS_GET_DEVICE_REV_ID_REQ_MSG +{ + USHORT Type; // QMUX type 0x0005 + USHORT Length; +} __attribute__ ((packed)) QMIDMS_GET_DEVICE_REV_ID_REQ_MSG, *PQMIDMS_GET_DEVICE_REV_ID_REQ_MSG; + +typedef struct _DEVICE_REV_ID +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR RevisionID; +} __attribute__ ((packed)) DEVICE_REV_ID, *PDEVICE_REV_ID; + +#if 0 +typedef struct _QMIDMS_GET_DEVICE_REV_ID_RESP_MSG +{ + USHORT Type; // QMUX type 0x0023 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMIDMS_GET_DEVICE_REV_ID_RESP_MSG, *PQMIDMS_GET_DEVICE_REV_ID_RESP_MSG; + +typedef struct _QMIDMS_GET_MSISDN_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; +} QMIDMS_GET_MSISDN_REQ_MSG, *PQMIDMS_GET_MSISDN_REQ_MSG; + +typedef struct _QCTLV_DEVICE_VOICE_NUMBERS +{ + UCHAR TLVType; // as defined above + USHORT TLVLength; // 4/7/7 + UCHAR VoideNumberString; // ESN, IMEI, or MEID + +} QCTLV_DEVICE_VOICE_NUMBERS, *PQCTLV_DEVICE_VOICE_NUMBERS; + + +typedef struct _QMIDMS_GET_MSISDN_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG +} QMIDMS_GET_MSISDN_RESP_MSG, *PQMIDMS_GET_MSISDN_RESP_MSG; +#endif + +typedef struct _QMIDMS_UIM_GET_IMSI_REQ_MSG +{ + USHORT Type; + USHORT Length; +} __attribute__ ((packed)) QMIDMS_UIM_GET_IMSI_REQ_MSG, *PQMIDMS_UIM_GET_IMSI_REQ_MSG; + +typedef struct _QMIDMS_UIM_GET_IMSI_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR IMSI; +} __attribute__ ((packed)) QMIDMS_UIM_GET_IMSI_RESP_MSG, *PQMIDMS_UIM_GET_IMSI_RESP_MSG; + +#if 0 +typedef struct _QMIDMS_GET_DEVICE_SERIAL_NUMBERS_REQ_MSG +{ + USHORT Type; // QMUX type 0x0007 + USHORT Length; +} QMIDMS_GET_DEVICE_SERIAL_NUMBERS_REQ_MSG, *PQMIDMS_GET_DEVICE_SERIAL_NUMBERS_REQ_MSG; + +#define QCTLV_TYPE_SER_NUM_ESN 0x10 +#define QCTLV_TYPE_SER_NUM_IMEI 0x11 +#define QCTLV_TYPE_SER_NUM_MEID 0x12 + +typedef struct _QCTLV_DEVICE_SERIAL_NUMBER +{ + UCHAR TLVType; // as defined above + USHORT TLVLength; // 4/7/7 + UCHAR SerialNumberString; // ESN, IMEI, or MEID + +} QCTLV_DEVICE_SERIAL_NUMBER, *PQCTLV_DEVICE_SERIAL_NUMBER; + +typedef struct _QMIDMS_GET_DEVICE_SERIAL_NUMBERS_RESP_MSG +{ + USHORT Type; // QMUX type 0x0007 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + // followed by optional TLV +} QMIDMS_GET_DEVICE_SERIAL_NUMBERS_RESP_MSG, *PQMIDMS_GET_DEVICE_SERIAL_NUMBERS_RESP; + +typedef struct _QMIDMS_GET_DMS_BAND_CAP +{ + USHORT Type; + USHORT Length; +} QMIDMS_GET_BAND_CAP_REQ_MSG, *PQMIDMS_GET_BAND_CAP_REQ_MSG; + +typedef struct _QMIDMS_GET_BAND_CAP_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_NONE + // QMI_ERR_INTERNAL + // QMI_ERR_MALFORMED_MSG + // QMI_ERR_NO_MEMORY + + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 2 + ULONG64 BandCap; +} QMIDMS_GET_BAND_CAP_RESP_MSG, *PQMIDMS_GET_BAND_CAP_RESP; + +typedef struct _QMIDMS_GET_DEVICE_CAP_REQ_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; +} QMIDMS_GET_DEVICE_CAP_REQ_MSG, *PQMIDMS_GET_DEVICE_CAP_REQ_MSG; + +typedef struct _QMIDMS_GET_DEVICE_CAP_RESP_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMUX_RESULT_SUCCESS + // QMUX_RESULT_FAILURE + USHORT QMUXError; // QMUX_ERR_INVALID_ARG + // QMUX_ERR_NO_MEMORY + // QMUX_ERR_INTERNAL + // QMUX_ERR_FAULT + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 2 + + ULONG MaxTxChannelRate; + ULONG MaxRxChannelRate; + UCHAR VoiceCap; + UCHAR SimCap; + + UCHAR RadioIfListCnt; // #elements in radio interface list + UCHAR RadioIfList; // N 1-byte elements +} QMIDMS_GET_DEVICE_CAP_RESP_MSG, *PQMIDMS_GET_DEVICE_CAP_RESP_MSG; + +typedef struct _QMIDMS_GET_ACTIVATED_STATUS_REQ_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; +} QMIDMS_GET_ACTIVATED_STATUS_REQ_MSG, *PQMIDMS_GET_ACTIVATES_STATUD_REQ_MSG; + +typedef struct _QMIDMS_GET_ACTIVATED_STATUS_RESP_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMUX_RESULT_SUCCESS + // QMUX_RESULT_FAILURE + USHORT QMUXError; // QMUX_ERR_INVALID_ARG + // QMUX_ERR_NO_MEMORY + // QMUX_ERR_INTERNAL + // QMUX_ERR_FAULT + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 2 + + USHORT ActivatedStatus; +} QMIDMS_GET_ACTIVATED_STATUS_RESP_MSG, *PQMIDMS_GET_ACTIVATED_STATUS_RESP_MSG; + +typedef struct _QMIDMS_GET_OPERATING_MODE_REQ_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; +} QMIDMS_GET_OPERATING_MODE_REQ_MSG, *PQMIDMS_GET_OPERATING_MODE_REQ_MSG; + +typedef struct _OFFLINE_REASON +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT OfflineReason; +} OFFLINE_REASON, *POFFLINE_REASON; + +typedef struct _HARDWARE_RESTRICTED_MODE +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR HardwareControlledMode; +} HARDWARE_RESTRICTED_MODE, *PHARDWARE_RESTRICTED_MODE; + +typedef struct _QMIDMS_GET_OPERATING_MODE_RESP_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMUX_RESULT_SUCCESS + // QMUX_RESULT_FAILURE + USHORT QMUXError; // QMUX_ERR_INVALID_ARG + // QMUX_ERR_NO_MEMORY + // QMUX_ERR_INTERNAL + // QMUX_ERR_FAULT + UCHAR TLV2Type; // 0x01 + USHORT TLV2Length; // 2 + + UCHAR OperatingMode; +} QMIDMS_GET_OPERATING_MODE_RESP_MSG, *PQMIDMS_GET_OPERATING_MODE_RESP_MSG; + +typedef struct _QMIDMS_UIM_GET_ICCID_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; +} QMIDMS_UIM_GET_ICCID_REQ_MSG, *PQMIDMS_UIM_GET_ICCID_REQ_MSG; + +typedef struct _QMIDMS_UIM_GET_ICCID_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; // 0x01 - required parameter + USHORT TLV2Length; // var + UCHAR ICCID; // String of voice number +} QMIDMS_UIM_GET_ICCID_RESP_MSG, *PQMIDMS_UIM_GET_ICCID_RESP_MSG; +#endif + +typedef struct _QMIDMS_SET_OPERATING_MODE_REQ_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + UCHAR OperatingMode; +} __attribute__ ((packed)) QMIDMS_SET_OPERATING_MODE_REQ_MSG, *PQMIDMS_SET_OPERATING_MODE_REQ_MSG; + +typedef struct _QMIDMS_SET_OPERATING_MODE_RESP_MSG +{ + USHORT Type; // QMUX type 0x0002 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMUX_RESULT_SUCCESS + // QMUX_RESULT_FAILURE + USHORT QMUXError; // QMUX_ERR_INVALID_ARG + // QMUX_ERR_NO_MEMORY + // QMUX_ERR_INTERNAL + // QMUX_ERR_FAULT +} __attribute__ ((packed)) QMIDMS_SET_OPERATING_MODE_RESP_MSG, *PQMIDMS_SET_OPERATING_MODE_RESP_MSG; + +#if 0 +typedef struct _QMIDMS_ACTIVATE_AUTOMATIC_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // + UCHAR ActivateCodelen; + UCHAR ActivateCode; +} QMIDMS_ACTIVATE_AUTOMATIC_REQ_MSG, *PQMIDMS_ACTIVATE_AUTOMATIC_REQ_MSG; + +typedef struct _QMIDMS_ACTIVATE_AUTOMATIC_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMIDMS_ACTIVATE_AUTOMATIC_RESP_MSG, *PQMIDMS_ACTIVATE_AUTOMATIC_RESP_MSG; + + +typedef struct _SPC_MSG +{ + UCHAR SPC[6]; + USHORT SID; +} SPC_MSG, *PSPC_MSG; + +typedef struct _MDN_MSG +{ + UCHAR MDNLEN; + UCHAR MDN; +} MDN_MSG, *PMDN_MSG; + +typedef struct _MIN_MSG +{ + UCHAR MINLEN; + UCHAR MIN; +} MIN_MSG, *PMIN_MSG; + +typedef struct _PRL_MSG +{ + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // + USHORT PRLLEN; + UCHAR PRL; +} PRL_MSG, *PPRL_MSG; + +typedef struct _MN_HA_KEY_MSG +{ + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // + UCHAR MN_HA_KEY_LEN; + UCHAR MN_HA_KEY; +} MN_HA_KEY_MSG, *PMN_HA_KEY_MSG; + +typedef struct _MN_AAA_KEY_MSG +{ + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // + UCHAR MN_AAA_KEY_LEN; + UCHAR MN_AAA_KEY; +} MN_AAA_KEY_MSG, *PMN_AAA_KEY_MSG; + +typedef struct _QMIDMS_ACTIVATE_MANUAL_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // + UCHAR Value; +} QMIDMS_ACTIVATE_MANUAL_REQ_MSG, *PQMIDMS_ACTIVATE_MANUAL_REQ_MSG; + +typedef struct _QMIDMS_ACTIVATE_MANUAL_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMIDMS_ACTIVATE_MANUAL_RESP_MSG, *PQMIDMS_ACTIVATE_MANUAL_RESP_MSG; +#endif + +typedef struct _QMIDMS_UIM_GET_STATE_REQ_MSG +{ + USHORT Type; + USHORT Length; +} __attribute__ ((packed)) QMIDMS_UIM_GET_STATE_REQ_MSG, *PQMIDMS_UIM_GET_STATE_REQ_MSG; + +typedef struct _QMIDMS_UIM_GET_STATE_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR UIMState; +} __attribute__ ((packed)) QMIDMS_UIM_GET_STATE_RESP_MSG, *PQMIDMS_UIM_GET_STATE_RESP_MSG; + +typedef struct _QMIDMS_UIM_GET_PIN_STATUS_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; +} __attribute__ ((packed)) QMIDMS_UIM_GET_PIN_STATUS_REQ_MSG, *PQMIDMS_UIM_GET_PIN_STATUS_REQ_MSG; + +typedef struct _QMIDMS_UIM_PIN_STATUS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR PINStatus; + UCHAR PINVerifyRetriesLeft; + UCHAR PINUnblockRetriesLeft; +} __attribute__ ((packed)) QMIDMS_UIM_PIN_STATUS, *PQMIDMS_UIM_PIN_STATUS; + +#define QMI_PIN_STATUS_NOT_INIT 0 +#define QMI_PIN_STATUS_NOT_VERIF 1 +#define QMI_PIN_STATUS_VERIFIED 2 +#define QMI_PIN_STATUS_DISABLED 3 +#define QMI_PIN_STATUS_BLOCKED 4 +#define QMI_PIN_STATUS_PERM_BLOCKED 5 +#define QMI_PIN_STATUS_UNBLOCKED 6 +#define QMI_PIN_STATUS_CHANGED 7 + + +typedef struct _QMIDMS_UIM_GET_PIN_STATUS_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR PinStatus; +} __attribute__ ((packed)) QMIDMS_UIM_GET_PIN_STATUS_RESP_MSG, *PQMIDMS_UIM_GET_PIN_STATUS_RESP_MSG; + +#if 0 +typedef struct _QMIDMS_UIM_GET_CK_STATUS_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR Facility; +} QMIDMS_UIM_GET_CK_STATUS_REQ_MSG, *PQMIDMS_UIM_GET_CK_STATUS_REQ_MSG; + + +typedef struct _QMIDMS_UIM_CK_STATUS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR FacilityStatus; + UCHAR FacilityVerifyRetriesLeft; + UCHAR FacilityUnblockRetriesLeft; +} QMIDMS_UIM_CK_STATUS, *PQMIDMS_UIM_CK_STATUS; + +typedef struct _QMIDMS_UIM_CK_OPERATION_STATUS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR OperationBlocking; +} QMIDMS_UIM_CK_OPERATION_STATUS, *PQMIDMS_UIM_CK_OPERATION_STATUS; + +typedef struct _QMIDMS_UIM_GET_CK_STATUS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR CkStatus; +} QMIDMS_UIM_GET_CK_STATUS_RESP_MSG, *PQMIDMS_UIM_GET_CK_STATUS_RESP_MSG; +#endif + +typedef struct _QMIDMS_UIM_VERIFY_PIN_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + UCHAR PINID; + UCHAR PINLen; + UCHAR PINValue; +} __attribute__ ((packed)) QMIDMS_UIM_VERIFY_PIN_REQ_MSG, *PQMIDMS_UIM_VERIFY_PIN_REQ_MSG; + +typedef struct _QMIDMS_UIM_VERIFY_PIN_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR PINVerifyRetriesLeft; + UCHAR PINUnblockRetriesLeft; +} __attribute__ ((packed)) QMIDMS_UIM_VERIFY_PIN_RESP_MSG, *PQMIDMS_UIM_VERIFY_PIN_RESP_MSG; + +#if 0 +typedef struct _QMIDMS_UIM_SET_PIN_PROTECTION_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + UCHAR PINID; + UCHAR ProtectionSetting; + UCHAR PINLen; + UCHAR PINValue; +} QMIDMS_UIM_SET_PIN_PROTECTION_REQ_MSG, *PQMIDMS_UIM_SET_PIN_PROTECTION_REQ_MSG; + +typedef struct _QMIDMS_UIM_SET_PIN_PROTECTION_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR PINVerifyRetriesLeft; + UCHAR PINUnblockRetriesLeft; +} QMIDMS_UIM_SET_PIN_PROTECTION_RESP_MSG, *PQMIDMS_UIM_SET_PIN_PROTECTION_RESP_MSG; + +typedef struct _QMIDMS_UIM_SET_CK_PROTECTION_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR Facility; + UCHAR FacilityState; + UCHAR FacliltyLen; + UCHAR FacliltyValue; +} QMIDMS_UIM_SET_CK_PROTECTION_REQ_MSG, *PQMIDMS_UIM_SET_CK_PROTECTION_REQ_MSG; + +typedef struct _QMIDMS_UIM_SET_CK_PROTECTION_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR FacilityRetriesLeft; +} QMIDMS_UIM_SET_CK_PROTECTION_RESP_MSG, *PQMIDMS_UIM_SET_CK_PROTECTION_RESP_MSG; + + +typedef struct _UIM_PIN +{ + UCHAR PinLength; + UCHAR PinValue; +} UIM_PIN, *PUIM_PIN; + +typedef struct _QMIDMS_UIM_CHANGE_PIN_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + UCHAR PINID; + UCHAR PinDetails; +} QMIDMS_UIM_CHANGE_PIN_REQ_MSG, *PQMIDMS_UIM_CHANGE_PIN_REQ_MSG; + +typedef struct QMIDMS_UIM_CHANGE_PIN_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR PINVerifyRetriesLeft; + UCHAR PINUnblockRetriesLeft; +} QMIDMS_UIM_CHANGE_PIN_RESP_MSG, *PQMIDMS_UIM_CHANGE_PIN_RESP_MSG; + +typedef struct _UIM_PUK +{ + UCHAR PukLength; + UCHAR PukValue; +} UIM_PUK, *PUIM_PUK; + +typedef struct _QMIDMS_UIM_UNBLOCK_PIN_REQ_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + UCHAR PINID; + UCHAR PinDetails; +} QMIDMS_UIM_UNBLOCK_PIN_REQ_MSG, *PQMIDMS_UIM_BLOCK_PIN_REQ_MSG; + +typedef struct QMIDMS_UIM_UNBLOCK_PIN_RESP_MSG +{ + USHORT Type; // QMUX type 0x0024 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR PINVerifyRetriesLeft; + UCHAR PINUnblockRetriesLeft; +} QMIDMS_UIM_UNBLOCK_PIN_RESP_MSG, *PQMIDMS_UIM_UNBLOCK_PIN_RESP_MSG; + +typedef struct _QMIDMS_UIM_UNBLOCK_CK_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR Facility; + UCHAR FacliltyUnblockLen; + UCHAR FacliltyUnblockValue; +} QMIDMS_UIM_UNBLOCK_CK_REQ_MSG, *PQMIDMS_UIM_BLOCK_CK_REQ_MSG; + +typedef struct QMIDMS_UIM_UNBLOCK_CK_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR FacilityUnblockRetriesLeft; +} QMIDMS_UIM_UNBLOCK_CK_RESP_MSG, *PQMIDMS_UIM_UNBLOCK_CK_RESP_MSG; + +typedef struct _QMIDMS_SET_EVENT_REPORT_REQ_MSG +{ + USHORT Type; + USHORT Length; +} QMIDMS_SET_EVENT_REPORT_REQ_MSG, *PQMIDMS_SET_EVENT_REPORT_REQ_MSG; + +typedef struct _QMIDMS_SET_EVENT_REPORT_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG +} QMIDMS_SET_EVENT_REPORT_RESP_MSG, *PQMIDMS_SET_EVENT_REPORT_RESP_MSG; + +typedef struct _PIN_STATUS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ReportPinState; +} PIN_STATUS, *PPIN_STATUS; + +typedef struct _POWER_STATUS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR PowerStatus; + UCHAR BatteryLvl; +} POWER_STATUS, *PPOWER_STATUS; + +typedef struct _ACTIVATION_STATE +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT ActivationState; +} ACTIVATION_STATE, *PACTIVATION_STATE; + +typedef struct _ACTIVATION_STATE_REQ +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ActivationState; +} ACTIVATION_STATE_REQ, *PACTIVATION_STATE_REQ; + +typedef struct _OPERATING_MODE +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR OperatingMode; +} OPERATING_MODE, *POPERATING_MODE; + +typedef struct _UIM_STATE +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR UIMState; +} UIM_STATE, *PUIM_STATE; + +typedef struct _WIRELESS_DISABLE_STATE +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR WirelessDisableState; +} WIRELESS_DISABLE_STATE, *PWIRELESS_DISABLE_STATE; + +typedef struct _QMIDMS_EVENT_REPORT_IND_MSG +{ + USHORT Type; + USHORT Length; +} QMIDMS_EVENT_REPORT_IND_MSG, *PQMIDMS_EVENT_REPORT_IND_MSG; +#endif + +// ============================ END OF DMS =============================== + +// ======================= QOS ============================== +typedef struct _MPIOC_DEV_INFO MPIOC_DEV_INFO, *PMPIOC_DEV_INFO; + +#define QMI_QOS_SET_EVENT_REPORT_REQ 0x0001 +#define QMI_QOS_SET_EVENT_REPORT_RESP 0x0001 +#define QMI_QOS_EVENT_REPORT_IND 0x0001 + +#if 0 +typedef struct _QMI_QOS_SET_EVENT_REPORT_REQ_MSG +{ + USHORT Type; // QMUX type 0x0001 + USHORT Length; + // UCHAR TLVType; // 0x01 - physical link state + // USHORT TLVLength; // 1 + // UCHAR PhyLinkStatusRpt; // 0-enable; 1-disable + UCHAR TLVType2; // 0x02 = global flow reporting + USHORT TLVLength2; // 1 + UCHAR GlobalFlowRpt; // 1-enable; 0-disable +} QMI_QOS_SET_EVENT_REPORT_REQ_MSG, *PQMI_QOS_SET_EVENT_REPORT_REQ_MSG; + +typedef struct _QMI_QOS_SET_EVENT_REPORT_RESP_MSG +{ + USHORT Type; // QMUX type 0x0010 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMUX_RESULT_SUCCESS + // QMUX_RESULT_FAILURE + USHORT QMUXError; // QMUX_ERR_INVALID_ARG + // QMUX_ERR_NO_MEMORY + // QMUX_ERR_INTERNAL + // QMUX_ERR_FAULT +} QMI_QOS_SET_EVENT_REPORT_RESP_MSG, *PQMI_QOS_SET_EVENT_REPORT_RESP_MSG; + +typedef struct _QMI_QOS_EVENT_REPORT_IND_MSG +{ + USHORT Type; // QMUX type 0x0001 + USHORT Length; + UCHAR TLVs; +} QMI_QOS_EVENT_REPORT_IND_MSG, *PQMI_QOS_EVENT_REPORT_IND_MSG; + +#define QOS_EVENT_RPT_IND_FLOW_ACTIVATED 0x01 +#define QOS_EVENT_RPT_IND_FLOW_MODIFIED 0x02 +#define QOS_EVENT_RPT_IND_FLOW_DELETED 0x03 +#define QOS_EVENT_RPT_IND_FLOW_SUSPENDED 0x04 +#define QOS_EVENT_RPT_IND_FLOW_ENABLED 0x05 +#define QOS_EVENT_RPT_IND_FLOW_DISABLED 0x06 + +#define QOS_EVENT_RPT_IND_TLV_PHY_LINK_STATE_TYPE 0x01 +#define QOS_EVENT_RPT_IND_TLV_GLOBAL_FL_RPT_STATE 0x10 +#define QOS_EVENT_RPT_IND_TLV_GLOBAL_FL_RPT_TYPE 0x10 +#define QOS_EVENT_RPT_IND_TLV_TX_FLOW_TYPE 0x11 +#define QOS_EVENT_RPT_IND_TLV_RX_FLOW_TYPE 0x12 +#define QOS_EVENT_RPT_IND_TLV_TX_FILTER_TYPE 0x13 +#define QOS_EVENT_RPT_IND_TLV_RX_FILTER_TYPE 0x14 +#define QOS_EVENT_RPT_IND_TLV_FLOW_SPEC 0x10 +#define QOS_EVENT_RPT_IND_TLV_FILTER_SPEC 0x10 + +typedef struct _QOS_EVENT_RPT_IND_TLV_PHY_LINK_STATE +{ + UCHAR TLVType; // 0x01 + USHORT TLVLength; // 1 + UCHAR PhyLinkState; // 0-dormant, 1-active +} QOS_EVENT_RPT_IND_TLV_PHY_LINK_STATE, *PQOS_EVENT_RPT_IND_TLV_PHY_LINK_STATE; + +typedef struct _QOS_EVENT_RPT_IND_TLV_GLOBAL_FL_RPT +{ + UCHAR TLVType; // 0x10 + USHORT TLVLength; // 6 + ULONG QosId; + UCHAR NewFlow; // 1: newly added flow; 0: existing flow + UCHAR StateChange; // 1: activated; 2: modified; 3: deleted; + // 4: suspended(delete); 5: enabled; 6: disabled +} QOS_EVENT_RPT_IND_TLV_GLOBAL_FL_RPT, *PQOS_EVENT_RPT_IND_TLV_GLOBAL_FL_RPT; + +// QOS Flow + +typedef struct _QOS_EVENT_RPT_IND_TLV_FLOW +{ + UCHAR TLVType; // 0x10-TX flow; 0x11-RX flow + USHORT TLVLength; // var + // embedded TLV's +} QOS_EVENT_RPT_IND_TLV_TX_FLOW, *PQOS_EVENT_RPT_IND_TLV_TX_FLOW; + +#define QOS_FLOW_TLV_IP_FLOW_IDX_TYPE 0x10 +#define QOS_FLOW_TLV_IP_FLOW_TRAFFIC_CLASS_TYPE 0x11 +#define QOS_FLOW_TLV_IP_FLOW_DATA_RATE_MIN_MAX_TYPE 0x12 +#define QOS_FLOW_TLV_IP_FLOW_DATA_RATE_TOKEN_BUCKET_TYPE 0x13 +#define QOS_FLOW_TLV_IP_FLOW_LATENCY_TYPE 0x14 +#define QOS_FLOW_TLV_IP_FLOW_JITTER_TYPE 0x15 +#define QOS_FLOW_TLV_IP_FLOW_PKT_ERR_RATE_TYPE 0x16 +#define QOS_FLOW_TLV_IP_FLOW_MIN_PKT_SIZE_TYPE 0x17 +#define QOS_FLOW_TLV_IP_FLOW_MAX_PKT_SIZE_TYPE 0x18 +#define QOS_FLOW_TLV_IP_FLOW_3GPP_BIT_ERR_RATE_TYPE 0x19 +#define QOS_FLOW_TLV_IP_FLOW_3GPP_TRAF_PRIORITY_TYPE 0x1A +#define QOS_FLOW_TLV_IP_FLOW_3GPP2_PROFILE_ID_TYPE 0x1B + +typedef struct _QOS_FLOW_TLV_IP_FLOW_IDX +{ + UCHAR TLVType; // 0x10 + USHORT TLVLength; // 1 + UCHAR IpFlowIndex; +} QOS_FLOW_TLV_IP_FLOW_IDX, *PQOS_FLOW_TLV_IP_FLOW_IDX; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_TRAFFIC_CLASS +{ + UCHAR TLVType; // 0x11 + USHORT TLVLength; // 1 + UCHAR TrafficClass; +} QOS_FLOW_TLV_IP_FLOW_TRAFFIC_CLASS, *PQOS_FLOW_TLV_IP_FLOW_TRAFFIC_CLASS; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_DATA_RATE_MIN_MAX +{ + UCHAR TLVType; // 0x12 + USHORT TLVLength; // 8 + ULONG DataRateMax; + ULONG GuaranteedRate; +} QOS_FLOW_TLV_IP_FLOW_DATA_RATE_MIN_MAX, *PQOS_FLOW_TLV_IP_FLOW_DATA_RATE_MIN_MAX; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_DATA_RATE_TOKEN_BUCKET +{ + UCHAR TLVType; // 0x13 + USHORT TLVLength; // 12 + ULONG PeakRate; + ULONG TokenRate; + ULONG BucketSize; +} QOS_FLOW_TLV_IP_FLOW_DATA_RATE_TOKEN_BUCKET, *PQOS_FLOW_TLV_IP_FLOW_DATA_RATE_TOKEN_BUCKET; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_LATENCY +{ + UCHAR TLVType; // 0x14 + USHORT TLVLength; // 4 + ULONG IpFlowLatency; +} QOS_FLOW_TLV_IP_FLOW_LATENCY, *PQOS_FLOW_TLV_IP_FLOW_LATENCY; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_JITTER +{ + UCHAR TLVType; // 0x15 + USHORT TLVLength; // 4 + ULONG IpFlowJitter; +} QOS_FLOW_TLV_IP_FLOW_JITTER, *PQOS_FLOW_TLV_IP_FLOW_JITTER; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_PKT_ERR_RATE +{ + UCHAR TLVType; // 0x16 + USHORT TLVLength; // 4 + USHORT ErrRateMultiplier; + USHORT ErrRateExponent; +} QOS_FLOW_TLV_IP_FLOW_PKT_ERR_RATE, *PQOS_FLOW_TLV_IP_FLOW_PKT_ERR_RATE; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_MIN_PKT_SIZE +{ + UCHAR TLVType; // 0x17 + USHORT TLVLength; // 4 + ULONG MinPolicedPktSize; +} QOS_FLOW_TLV_IP_FLOW_MIN_PKT_SIZE, *PQOS_FLOW_TLV_IP_FLOW_MIN_PKT_SIZE; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_MAX_PKT_SIZE +{ + UCHAR TLVType; // 0x18 + USHORT TLVLength; // 4 + ULONG MaxAllowedPktSize; +} QOS_FLOW_TLV_IP_FLOW_MAX_PKT_SIZE, *PQOS_FLOW_TLV_IP_FLOW_MAX_PKT_SIZE; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_3GPP_BIT_ERR_RATE +{ + UCHAR TLVType; // 0x19 + USHORT TLVLength; // 1 + UCHAR ResidualBitErrorRate; +} QOS_FLOW_TLV_IP_FLOW_3GPP_BIT_ERR_RATE, *PQOS_FLOW_TLV_IP_FLOW_3GPP_BIT_ERR_RATE; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_3GPP_TRAF_PRIORITY +{ + UCHAR TLVType; // 0x1A + USHORT TLVLength; // 1 + UCHAR TrafficHandlingPriority; +} QOS_FLOW_TLV_IP_FLOW_3GPP_TRAF_PRIORITY, *PQOS_FLOW_TLV_IP_FLOW_3GPP_TRAF_PRIORITY; + +typedef struct _QOS_FLOW_TLV_IP_FLOW_3GPP2_PROFILE_ID +{ + UCHAR TLVType; // 0x1B + USHORT TLVLength; // 2 + USHORT ProfileId; +} QOS_FLOW_TLV_IP_FLOW_3GPP2_PROFILE_ID, *PQOS_FLOW_TLV_IP_FLOW_3GPP2_PROFILE_ID; + +// QOS Filter + +#define QOS_FILTER_TLV_IP_FILTER_IDX_TYPE 0x10 +#define QOS_FILTER_TLV_IP_VERSION_TYPE 0x11 +#define QOS_FILTER_TLV_IPV4_SRC_ADDR_TYPE 0x12 +#define QOS_FILTER_TLV_IPV4_DEST_ADDR_TYPE 0x13 +#define QOS_FILTER_TLV_NEXT_HDR_PROTOCOL_TYPE 0x14 +#define QOS_FILTER_TLV_IPV4_TYPE_OF_SERVICE_TYPE 0x15 +#define QOS_FILTER_TLV_TCP_UDP_PORT_SRC_TCP_TYPE 0x1B +#define QOS_FILTER_TLV_TCP_UDP_PORT_DEST_TCP_TYPE 0x1C +#define QOS_FILTER_TLV_TCP_UDP_PORT_SRC_UDP_TYPE 0x1D +#define QOS_FILTER_TLV_TCP_UDP_PORT_DEST_UDP_TYPE 0x1E +#define QOS_FILTER_TLV_ICMP_FILTER_MSG_TYPE_TYPE 0x1F +#define QOS_FILTER_TLV_ICMP_FILTER_MSG_CODE_TYPE 0x20 +#define QOS_FILTER_TLV_TCP_UDP_PORT_SRC_TYPE 0x24 +#define QOS_FILTER_TLV_TCP_UDP_PORT_DEST_TYPE 0x25 + +typedef struct _QOS_EVENT_RPT_IND_TLV_FILTER +{ + UCHAR TLVType; // 0x12-TX filter; 0x13-RX filter + USHORT TLVLength; // var + // embedded TLV's +} QOS_EVENT_RPT_IND_TLV_RX_FILTER, *PQOS_EVENT_RPT_IND_TLV_RX_FILTER; + +typedef struct _QOS_FILTER_TLV_IP_FILTER_IDX +{ + UCHAR TLVType; // 0x10 + USHORT TLVLength; // 1 + UCHAR IpFilterIndex; +} QOS_FILTER_TLV_IP_FILTER_IDX, *PQOS_FILTER_TLV_IP_FILTER_IDX; + +typedef struct _QOS_FILTER_TLV_IP_VERSION +{ + UCHAR TLVType; // 0x11 + USHORT TLVLength; // 1 + UCHAR IpVersion; +} QOS_FILTER_TLV_IP_VERSION, *PQOS_FILTER_TLV_IP_VERSION; + +typedef struct _QOS_FILTER_TLV_IPV4_SRC_ADDR +{ + UCHAR TLVType; // 0x12 + USHORT TLVLength; // 8 + ULONG IpSrcAddr; + ULONG IpSrcSubnetMask; +} QOS_FILTER_TLV_IPV4_SRC_ADDR, *PQOS_FILTER_TLV_IPV4_SRC_ADDR; + +typedef struct _QOS_FILTER_TLV_IPV4_DEST_ADDR +{ + UCHAR TLVType; // 0x13 + USHORT TLVLength; // 8 + ULONG IpDestAddr; + ULONG IpDestSubnetMask; +} QOS_FILTER_TLV_IPV4_DEST_ADDR, *PQOS_FILTER_TLV_IPV4_DEST_ADDR; + +typedef struct _QOS_FILTER_TLV_NEXT_HDR_PROTOCOL +{ + UCHAR TLVType; // 0x14 + USHORT TLVLength; // 1 + UCHAR NextHdrProtocol; +} QOS_FILTER_TLV_NEXT_HDR_PROTOCOL, *PQOS_FILTER_TLV_NEXT_HDR_PROTOCOL; + +typedef struct _QOS_FILTER_TLV_IPV4_TYPE_OF_SERVICE +{ + UCHAR TLVType; // 0x15 + USHORT TLVLength; // 2 + UCHAR Ipv4TypeOfService; + UCHAR Ipv4TypeOfServiceMask; +} QOS_FILTER_TLV_IPV4_TYPE_OF_SERVICE, *PQOS_FILTER_TLV_IPV4_TYPE_OF_SERVICE; + +typedef struct _QOS_FILTER_TLV_TCP_UDP_PORT +{ + UCHAR TLVType; // source port: 0x1B-TCP; 0x1D-UDP + // dest port: 0x1C-TCP; 0x1E-UDP + USHORT TLVLength; // 4 + USHORT FilterPort; + USHORT FilterPortRange; +} QOS_FILTER_TLV_TCP_UDP_PORT, *PQOS_FILTER_TLV_TCP_UDP_PORT; + +typedef struct _QOS_FILTER_TLV_ICMP_FILTER_MSG_TYPE +{ + UCHAR TLVType; // 0x1F + USHORT TLVLength; // 1 + UCHAR IcmpFilterMsgType; +} QOS_FILTER_TLV_ICMP_FILTER_MSG_TYPE, *PQOS_FILTER_TLV_ICMP_FILTER_MSG_TYPE; + +typedef struct _QOS_FILTER_TLV_ICMP_FILTER_MSG_CODE +{ + UCHAR TLVType; // 0x20 + USHORT TLVLength; // 1 + UCHAR IcmpFilterMsgCode; +} QOS_FILTER_TLV_ICMP_FILTER_MSG_CODE, *PQOS_FILTER_TLV_ICMP_FILTER_MSG_CODE; + +#define QOS_FILTER_PRECEDENCE_INVALID 256 +#define QOS_FILTER_TLV_PRECEDENCE_TYPE 0x22 +#define QOS_FILTER_TLV_ID_TYPE 0x23 + +typedef struct _QOS_FILTER_TLV_PRECEDENCE +{ + UCHAR TLVType; // 0x22 + USHORT TLVLength; // 2 + USHORT Precedence; // precedence of the filter +} QOS_FILTER_TLV_PRECEDENCE, *PQOS_FILTER_TLV_PRECEDENCE; + +typedef struct _QOS_FILTER_TLV_ID +{ + UCHAR TLVType; // 0x23 + USHORT TLVLength; // 2 + USHORT FilterId; // filter ID +} QOS_FILTER_TLV_ID, *PQOS_FILTER_TLV_ID; + +#ifdef QCQOS_IPV6 + +#define QOS_FILTER_TLV_IPV6_SRC_ADDR_TYPE 0x16 +#define QOS_FILTER_TLV_IPV6_DEST_ADDR_TYPE 0x17 +#define QOS_FILTER_TLV_IPV6_NEXT_HDR_PROTOCOL_TYPE 0x14 // same as IPV4 +#define QOS_FILTER_TLV_IPV6_TRAFFIC_CLASS_TYPE 0x19 +#define QOS_FILTER_TLV_IPV6_FLOW_LABEL_TYPE 0x1A + +typedef struct _QOS_FILTER_TLV_IPV6_SRC_ADDR +{ + UCHAR TLVType; // 0x16 + USHORT TLVLength; // 17 + UCHAR IpSrcAddr[16]; + UCHAR IpSrcAddrPrefixLen; // [0..128] +} QOS_FILTER_TLV_IPV6_SRC_ADDR, *PQOS_FILTER_TLV_IPV6_SRC_ADDR; + +typedef struct _QOS_FILTER_TLV_IPV6_DEST_ADDR +{ + UCHAR TLVType; // 0x17 + USHORT TLVLength; // 17 + UCHAR IpDestAddr[16]; + UCHAR IpDestAddrPrefixLen; // [0..128] +} QOS_FILTER_TLV_IPV6_DEST_ADDR, *PQOS_FILTER_TLV_IPV6_DEST_ADDR; + +#define QOS_FILTER_IPV6_NEXT_HDR_PROTOCOL_TCP 0x06 +#define QOS_FILTER_IPV6_NEXT_HDR_PROTOCOL_UDP 0x11 + +typedef struct _QOS_FILTER_TLV_IPV6_TRAFFIC_CLASS +{ + UCHAR TLVType; // 0x19 + USHORT TLVLength; // 2 + UCHAR TrafficClass; + UCHAR TrafficClassMask; // compare the first 6 bits only +} QOS_FILTER_TLV_IPV6_TRAFFIC_CLASS, *PQOS_FILTER_TLV_IPV6_TRAFFIC_CLASS; + +typedef struct _QOS_FILTER_TLV_IPV6_FLOW_LABEL +{ + UCHAR TLVType; // 0x1A + USHORT TLVLength; // 4 + ULONG FlowLabel; +} QOS_FILTER_TLV_IPV6_FLOW_LABEL, *PQOS_FILTER_TLV_IPV6_FLOW_LABEL; + +#endif // QCQOS_IPV6 +#endif + +// ======================= WMS ============================== +#define QMIWMS_SET_EVENT_REPORT_REQ 0x0001 +#define QMIWMS_SET_EVENT_REPORT_RESP 0x0001 +#define QMIWMS_EVENT_REPORT_IND 0x0001 +#define QMIWMS_RAW_SEND_REQ 0x0020 +#define QMIWMS_RAW_SEND_RESP 0x0020 +#define QMIWMS_RAW_WRITE_REQ 0x0021 +#define QMIWMS_RAW_WRITE_RESP 0x0021 +#define QMIWMS_RAW_READ_REQ 0x0022 +#define QMIWMS_RAW_READ_RESP 0x0022 +#define QMIWMS_MODIFY_TAG_REQ 0x0023 +#define QMIWMS_MODIFY_TAG_RESP 0x0023 +#define QMIWMS_DELETE_REQ 0x0024 +#define QMIWMS_DELETE_RESP 0x0024 +#define QMIWMS_GET_MESSAGE_PROTOCOL_REQ 0x0030 +#define QMIWMS_GET_MESSAGE_PROTOCOL_RESP 0x0030 +#define QMIWMS_LIST_MESSAGES_REQ 0x0031 +#define QMIWMS_LIST_MESSAGES_RESP 0x0031 +#define QMIWMS_GET_SMSC_ADDRESS_REQ 0x0034 +#define QMIWMS_GET_SMSC_ADDRESS_RESP 0x0034 +#define QMIWMS_SET_SMSC_ADDRESS_REQ 0x0035 +#define QMIWMS_SET_SMSC_ADDRESS_RESP 0x0035 +#define QMIWMS_GET_STORE_MAX_SIZE_REQ 0x0036 +#define QMIWMS_GET_STORE_MAX_SIZE_RESP 0x0036 + + +#define WMS_MESSAGE_PROTOCOL_CDMA 0x00 +#define WMS_MESSAGE_PROTOCOL_WCDMA 0x01 + +#if 0 +typedef struct _QMIWMS_GET_MESSAGE_PROTOCOL_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMIWMS_GET_MESSAGE_PROTOCOL_REQ_MSG, *PQMIWMS_GET_MESSAGE_PROTOCOL_REQ_MSG; + +typedef struct _QMIWMS_GET_MESSAGE_PROTOCOL_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR MessageProtocol; +} QMIWMS_GET_MESSAGE_PROTOCOL_RESP_MSG, *PQMIWMS_GET_MESSAGE_PROTOCOL_RESP_MSG; + +typedef struct _QMIWMS_GET_STORE_MAX_SIZE_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR StorageType; +} QMIWMS_GET_STORE_MAX_SIZE_REQ_MSG, *PQMIWMS_GET_STORE_MAX_SIZE_REQ_MSG; + +typedef struct _QMIWMS_GET_STORE_MAX_SIZE_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + ULONG MemStoreMaxSize; +} QMIWMS_GET_STORE_MAX_SIZE_RESP_MSG, *PQMIWMS_GET_STORE_MAX_SIZE_RESP_MSG; + +typedef struct _REQUEST_TAG +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR TagType; +} REQUEST_TAG, *PREQUEST_TAG; + +typedef struct _QMIWMS_LIST_MESSAGES_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR StorageType; +} QMIWMS_LIST_MESSAGES_REQ_MSG, *PQMIWMS_LIST_MESSAGES_REQ_MSG; + +typedef struct _QMIWMS_MESSAGE +{ + ULONG MessageIndex; + UCHAR TagType; +} QMIWMS_MESSAGE, *PQMIWMS_MESSAGE; + +typedef struct _QMIWMS_LIST_MESSAGES_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + ULONG NumMessages; +} QMIWMS_LIST_MESSAGES_RESP_MSG, *PQMIWMS_LIST_MESSAGES_RESP_MSG; + +typedef struct _QMIWMS_RAW_READ_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR StorageType; + ULONG MemoryIndex; +} QMIWMS_RAW_READ_REQ_MSG, *PQMIWMS_RAW_READ_REQ_MSG; + +typedef struct _QMIWMS_RAW_READ_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR TagType; + UCHAR Format; + USHORT MessageLength; + UCHAR Message; +} QMIWMS_RAW_READ_RESP_MSG, *PQMIWMS_RAW_READ_RESP_MSG; + +typedef struct _QMIWMS_MODIFY_TAG_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR StorageType; + ULONG MemoryIndex; + UCHAR TagType; +} QMIWMS_MODIFY_TAG_REQ_MSG, *PQMIWMS_MODIFY_TAG_REQ_MSG; + +typedef struct _QMIWMS_MODIFY_TAG_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMIWMS_MODIFY_TAG_RESP_MSG, *PQMIWMS_MODIFY_TAG_RESP_MSG; + +typedef struct _QMIWMS_RAW_SEND_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR SmsFormat; + USHORT SmsLength; + UCHAR SmsMessage; +} QMIWMS_RAW_SEND_REQ_MSG, *PQMIWMS_RAW_SEND_REQ_MSG; + +typedef struct _RAW_SEND_CAUSE_CODE +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT CauseCode; +} RAW_SEND_CAUSE_CODE, *PRAW_SEND_CAUSE_CODE; + + +typedef struct _QMIWMS_RAW_SEND_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMIWMS_RAW_SEND_RESP_MSG, *PQMIWMS_RAW_SEND_RESP_MSG; + + +typedef struct _WMS_DELETE_MESSAGE_INDEX +{ + UCHAR TLVType; + USHORT TLVLength; + ULONG MemoryIndex; +} WMS_DELETE_MESSAGE_INDEX, *PWMS_DELETE_MESSAGE_INDEX; + +typedef struct _WMS_DELETE_MESSAGE_TAG +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR MessageTag; +} WMS_DELETE_MESSAGE_TAG, *PWMS_DELETE_MESSAGE_TAG; + +typedef struct _QMIWMS_DELETE_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR StorageType; +} QMIWMS_DELETE_REQ_MSG, *PQMIWMS_DELETE_REQ_MSG; + +typedef struct _QMIWMS_DELETE_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMIWMS_DELETE_RESP_MSG, *PQMIWMS_DELETE_RESP_MSG; + + +typedef struct _QMIWMS_GET_SMSC_ADDRESS_REQ_MSG +{ + USHORT Type; + USHORT Length; +} QMIWMS_GET_SMSC_ADDRESS_REQ_MSG, *PQMIWMS_GET_SMSC_ADDRESS_REQ_MSG; + +typedef struct _QMIWMS_SMSC_ADDRESS +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SMSCAddressType[3]; + UCHAR SMSCAddressLength; + UCHAR SMSCAddressDigits; +} QMIWMS_SMSC_ADDRESS, *PQMIWMS_SMSC_ADDRESS; + + +typedef struct _QMIWMS_GET_SMSC_ADDRESS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR SMSCAddress; +} QMIWMS_GET_SMSC_ADDRESS_RESP_MSG, *PQMIWMS_GET_SMSC_ADDRESS_RESP_MSG; + +typedef struct _QMIWMS_SET_SMSC_ADDRESS_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR SMSCAddress; +} QMIWMS_SET_SMSC_ADDRESS_REQ_MSG, *PQMIWMS_SET_SMSC_ADDRESS_REQ_MSG; + +typedef struct _QMIWMS_SET_SMSC_ADDRESS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMIWMS_SET_SMSC_ADDRESS_RESP_MSG, *PQMIWMS_SET_SMSC_ADDRESS_RESP_MSG; + +typedef struct _QMIWMS_SET_EVENT_REPORT_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR ReportNewMessage; +} QMIWMS_SET_EVENT_REPORT_REQ_MSG, *PQMIWMS_SET_EVENT_REPORT_REQ_MSG; + +typedef struct _QMIWMS_SET_EVENT_REPORT_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMIWMS_SET_EVENT_REPORT_RESP_MSG, *PQMIWMS_SET_EVENT_REPORT_RESP_MSG; + +typedef struct _QMIWMS_EVENT_REPORT_IND_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR StorageType; + ULONG StorageIndex; +} QMIWMS_EVENT_REPORT_IND_MSG, *PQMIWMS_EVENT_REPORT_IND_MSG; +#endif + +// ======================= End of WMS ============================== + + +// ======================= NAS ============================== +#define QMINAS_SET_EVENT_REPORT_REQ 0x0002 +#define QMINAS_SET_EVENT_REPORT_RESP 0x0002 +#define QMINAS_EVENT_REPORT_IND 0x0002 +#define QMINAS_GET_SIGNAL_STRENGTH_REQ 0x0020 +#define QMINAS_GET_SIGNAL_STRENGTH_RESP 0x0020 +#define QMINAS_PERFORM_NETWORK_SCAN_REQ 0x0021 +#define QMINAS_PERFORM_NETWORK_SCAN_RESP 0x0021 +#define QMINAS_INITIATE_NW_REGISTER_REQ 0x0022 +#define QMINAS_INITIATE_NW_REGISTER_RESP 0x0022 +#define QMINAS_INITIATE_ATTACH_REQ 0x0023 +#define QMINAS_INITIATE_ATTACH_RESP 0x0023 +#define QMINAS_GET_SERVING_SYSTEM_REQ 0x0024 +#define QMINAS_GET_SERVING_SYSTEM_RESP 0x0024 +#define QMINAS_SERVING_SYSTEM_IND 0x0024 +#define QMINAS_GET_HOME_NETWORK_REQ 0x0025 +#define QMINAS_GET_HOME_NETWORK_RESP 0x0025 +#define QMINAS_GET_PREFERRED_NETWORK_REQ 0x0026 +#define QMINAS_GET_PREFERRED_NETWORK_RESP 0x0026 +#define QMINAS_SET_PREFERRED_NETWORK_REQ 0x0027 +#define QMINAS_SET_PREFERRED_NETWORK_RESP 0x0027 +#define QMINAS_GET_FORBIDDEN_NETWORK_REQ 0x0028 +#define QMINAS_GET_FORBIDDEN_NETWORK_RESP 0x0028 +#define QMINAS_SET_FORBIDDEN_NETWORK_REQ 0x0029 +#define QMINAS_SET_FORBIDDEN_NETWORK_RESP 0x0029 +#define QMINAS_SET_TECHNOLOGY_PREF_REQ 0x002A +#define QMINAS_SET_TECHNOLOGY_PREF_RESP 0x002A +#define QMINAS_GET_RF_BAND_INFO_REQ 0x0031 +#define QMINAS_GET_RF_BAND_INFO_RESP 0x0031 +#define QMINAS_GET_PLMN_NAME_REQ 0x0044 +#define QMINAS_GET_PLMN_NAME_RESP 0x0044 +#define QUECTEL_PACKET_TRANSFER_START_IND 0X100 +#define QUECTEL_PACKET_TRANSFER_END_IND 0X101 +#define QMINAS_GET_SYS_INFO_REQ 0x004D +#define QMINAS_GET_SYS_INFO_RESP 0x004D +#define QMINAS_SYS_INFO_IND 0x004D + +typedef struct _QMINAS_GET_HOME_NETWORK_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} __attribute__ ((packed)) QMINAS_GET_HOME_NETWORK_REQ_MSG, *PQMINAS_GET_HOME_NETWORK_REQ_MSG; + +typedef struct _HOME_NETWORK_SYSTEMID +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT SystemID; + USHORT NetworkID; +} __attribute__ ((packed)) HOME_NETWORK_SYSTEMID, *PHOME_NETWORK_SYSTEMID; + +typedef struct _HOME_NETWORK +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + UCHAR NetworkDesclen; + UCHAR NetworkDesc; +} __attribute__ ((packed)) HOME_NETWORK, *PHOME_NETWORK; + +#if 0 +typedef struct _HOME_NETWORK_EXT +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + UCHAR NetworkDescDisp; + UCHAR NetworkDescEncoding; + UCHAR NetworkDesclen; + UCHAR NetworkDesc; +} HOME_NETWORK_EXT, *PHOME_NETWORK_EXT; + +typedef struct _QMINAS_GET_HOME_NETWORK_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} QMINAS_GET_HOME_NETWORK_RESP_MSG, *PQMINAS_GET_HOME_NETWORK_RESP_MSG; + +typedef struct _QMINAS_GET_PREFERRED_NETWORK_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMINAS_GET_PREFERRED_NETWORK_REQ_MSG, *PQMINAS_GET_PREFERRED_NETWORK_REQ_MSG; + + +typedef struct _PREFERRED_NETWORK +{ + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + USHORT RadioAccess; +} PREFERRED_NETWORK, *PPREFERRED_NETWORK; + +typedef struct _QMINAS_GET_PREFERRED_NETWORK_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; // 0x01 - required parameter + USHORT TLV2Length; // length of the mfr string + USHORT NumPreferredNetwork; +} QMINAS_GET_PREFERRED_NETWORK_RESP_MSG, *PQMINAS_GET_PREFERRED_NETWORK_RESP_MSG; + +typedef struct _QMINAS_GET_FORBIDDEN_NETWORK_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMINAS_GET_FORBIDDEN_NETWORK_REQ_MSG, *PQMINAS_GET_FORBIDDEN_NETWORK_REQ_MSG; + +typedef struct _FORBIDDEN_NETWORK +{ + USHORT MobileCountryCode; + USHORT MobileNetworkCode; +} FORBIDDEN_NETWORK, *PFORBIDDEN_NETWORK; + +typedef struct _QMINAS_GET_FORBIDDEN_NETWORK_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; // 0x01 - required parameter + USHORT TLV2Length; // length of the mfr string + USHORT NumForbiddenNetwork; +} QMINAS_GET_FORBIDDEN_NETWORK_RESP_MSG, *PQMINAS_GET_FORBIDDEN_NETWORK_RESP_MSG; + +typedef struct _QMINAS_GET_SERVING_SYSTEM_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMINAS_GET_SERVING_SYSTEM_REQ_MSG, *PQMINAS_GET_SERVING_SYSTEM_REQ_MSG; + +typedef struct _QMINAS_ROAMING_INDICATOR_MSG +{ + UCHAR TLVType; // 0x01 - required parameter + USHORT TLVLength; // length of the mfr string + UCHAR RoamingIndicator; +} QMINAS_ROAMING_INDICATOR_MSG, *PQMINAS_ROAMING_INDICATOR_MSG; +#endif + +typedef struct _QMINAS_DATA_CAP +{ + UCHAR TLVType; // 0x01 - required parameter + USHORT TLVLength; // length of the mfr string + UCHAR DataCapListLen; + UCHAR DataCap; +} __attribute__ ((packed)) QMINAS_DATA_CAP, *PQMINAS_DATA_CAP; + +typedef struct _QMINAS_CURRENT_PLMN_MSG +{ + UCHAR TLVType; // 0x01 - required parameter + USHORT TLVLength; // length of the mfr string + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + UCHAR NetworkDesclen; + UCHAR NetworkDesc; +} __attribute__ ((packed)) QMINAS_CURRENT_PLMN_MSG, *PQMINAS_CURRENT_PLMN_MSG; + +typedef struct _QMINAS_GET_SERVING_SYSTEM_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} __attribute__ ((packed)) QMINAS_GET_SERVING_SYSTEM_RESP_MSG, *PQMINAS_GET_SERVING_SYSTEM_RESP_MSG; + +typedef struct _SERVING_SYSTEM +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR RegistrationState; + UCHAR CSAttachedState; + UCHAR PSAttachedState; + UCHAR RegistredNetwork; + UCHAR InUseRadioIF; + UCHAR RadioIF; +} __attribute__ ((packed)) SERVING_SYSTEM, *PSERVING_SYSTEM; + +typedef struct _QMINAS_GET_SYS_INFO_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} __attribute__ ((packed)) QMINAS_GET_SYS_INFO_RESP_MSG, *PQMINAS_GET_SYS_INFO_RESP_MSG; + +typedef struct _QMINAS_SYS_INFO_IND_MSG +{ + USHORT Type; + USHORT Length; +} __attribute__ ((packed)) QMINAS_SYS_INFO_IND_MSG, *PQMINAS_SYS_INFO_IND_MSG; + +typedef struct _SERVICE_STATUS_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvStatus; + UCHAR IsPrefDataPath; +} __attribute__ ((packed)) SERVICE_STATUS_INFO, *PSERVICE_STATUS_INFO; + +typedef struct _CDMA_SYSTEM_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvDomainValid; + UCHAR SrvDomain; + UCHAR SrvCapabilityValid; + UCHAR SrvCapability; + UCHAR RoamStatusValid; + UCHAR RoamStatus; + UCHAR IsSysForbiddenValid; + UCHAR IsSysForbidden; + UCHAR IsSysPrlMatchValid; + UCHAR IsSysPrlMatch; + UCHAR PRevInUseValid; + UCHAR PRevInUse; + UCHAR BSPRevValid; + UCHAR BSPRev; + UCHAR CCSSupportedValid; + UCHAR CCSSupported; + UCHAR CDMASysIdValid; + USHORT SID; + USHORT NID; + UCHAR BSInfoValid; + USHORT BaseID; + ULONG BaseLAT; + ULONG BaseLONG; + UCHAR PacketZoneValid; + USHORT PacketZone; + UCHAR NetworkIdValid; + UCHAR MCC[3]; + UCHAR MNC[3]; +} __attribute__ ((packed)) CDMA_SYSTEM_INFO, *PCDMA_SYSTEM_INFO; + +typedef struct _HDR_SYSTEM_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvDomainValid; + UCHAR SrvDomain; + UCHAR SrvCapabilityValid; + UCHAR SrvCapability; + UCHAR RoamStatusValid; + UCHAR RoamStatus; + UCHAR IsSysForbiddenValid; + UCHAR IsSysForbidden; + UCHAR IsSysPrlMatchValid; + UCHAR IsSysPrlMatch; + UCHAR HdrPersonalityValid; + UCHAR HdrPersonality; + UCHAR HdrActiveProtValid; + UCHAR HdrActiveProt; + UCHAR is856SysIdValid; + UCHAR is856SysId[16]; +} __attribute__ ((packed)) HDR_SYSTEM_INFO, *PHDR_SYSTEM_INFO; + +typedef struct _GSM_SYSTEM_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvDomainValid; + UCHAR SrvDomain; + UCHAR SrvCapabilityValid; + UCHAR SrvCapability; + UCHAR RoamStatusValid; + UCHAR RoamStatus; + UCHAR IsSysForbiddenValid; + UCHAR IsSysForbidden; + UCHAR LacValid; + USHORT Lac; + UCHAR CellIdValid; + ULONG CellId; + UCHAR RegRejectInfoValid; + UCHAR RejectSrvDomain; + UCHAR RejCause; + UCHAR NetworkIdValid; + UCHAR MCC[3]; + UCHAR MNC[3]; + UCHAR EgprsSuppValid; + UCHAR EgprsSupp; + UCHAR DtmSuppValid; + UCHAR DtmSupp; +} __attribute__ ((packed)) GSM_SYSTEM_INFO, *PGSM_SYSTEM_INFO; + +typedef struct _WCDMA_SYSTEM_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvDomainValid; + UCHAR SrvDomain; + UCHAR SrvCapabilityValid; + UCHAR SrvCapability; + UCHAR RoamStatusValid; + UCHAR RoamStatus; + UCHAR IsSysForbiddenValid; + UCHAR IsSysForbidden; + UCHAR LacValid; + USHORT Lac; + UCHAR CellIdValid; + ULONG CellId; + UCHAR RegRejectInfoValid; + UCHAR RejectSrvDomain; + UCHAR RejCause; + UCHAR NetworkIdValid; + UCHAR MCC[3]; + UCHAR MNC[3]; + UCHAR HsCallStatusValid; + UCHAR HsCallStatus; + UCHAR HsIndValid; + UCHAR HsInd; + UCHAR PscValid; + UCHAR Psc; +} __attribute__ ((packed)) WCDMA_SYSTEM_INFO, *PWCDMA_SYSTEM_INFO; + +typedef struct _LTE_SYSTEM_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvDomainValid; + UCHAR SrvDomain; + UCHAR SrvCapabilityValid; + UCHAR SrvCapability; + UCHAR RoamStatusValid; + UCHAR RoamStatus; + UCHAR IsSysForbiddenValid; + UCHAR IsSysForbidden; + UCHAR LacValid; + USHORT Lac; + UCHAR CellIdValid; + ULONG CellId; + UCHAR RegRejectInfoValid; + UCHAR RejectSrvDomain; + UCHAR RejCause; + UCHAR NetworkIdValid; + UCHAR MCC[3]; + UCHAR MNC[3]; + UCHAR TacValid; + USHORT Tac; +} __attribute__ ((packed)) LTE_SYSTEM_INFO, *PLTE_SYSTEM_INFO; + +typedef struct _TDSCDMA_SYSTEM_INFO +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SrvDomainValid; + UCHAR SrvDomain; + UCHAR SrvCapabilityValid; + UCHAR SrvCapability; + UCHAR RoamStatusValid; + UCHAR RoamStatus; + UCHAR IsSysForbiddenValid; + UCHAR IsSysForbidden; + UCHAR LacValid; + USHORT Lac; + UCHAR CellIdValid; + ULONG CellId; + UCHAR RegRejectInfoValid; + UCHAR RejectSrvDomain; + UCHAR RejCause; + UCHAR NetworkIdValid; + UCHAR MCC[3]; + UCHAR MNC[3]; + UCHAR HsCallStatusValid; + UCHAR HsCallStatus; + UCHAR HsIndValid; + UCHAR HsInd; + UCHAR CellParameterIdValid; + USHORT CellParameterId; + UCHAR CellBroadcastCapValid; + ULONG CellBroadcastCap; + UCHAR CsBarStatusValid; + ULONG CsBarStatus; + UCHAR PsBarStatusValid; + ULONG PsBarStatus; + UCHAR CipherDomainValid; + UCHAR CipherDomain; +} __attribute__ ((packed)) TDSCDMA_SYSTEM_INFO, *PTDSCDMA_SYSTEM_INFO; + +#if 0 +typedef struct _QMINAS_SERVING_SYSTEM_IND_MSG +{ + USHORT Type; + USHORT Length; +} QMINAS_SERVING_SYSTEM_IND_MSG, *PQMINAS_SERVING_SYSTEM_IND_MSG; + +typedef struct _QMINAS_SET_PREFERRED_NETWORK_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT NumPreferredNetwork; + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + USHORT RadioAccess; +} QMINAS_SET_PREFERRED_NETWORK_REQ_MSG, *PQMINAS_SET_PREFERRED_NETWORK_REQ_MSG; + +typedef struct _QMINAS_SET_PREFERRED_NETWORK_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_SET_PREFERRED_NETWORK_RESP_MSG, *PQMINAS_SET_PREFERRED_NETWORK_RESP_MSG; + +typedef struct _QMINAS_SET_FORBIDDEN_NETWORK_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT NumForbiddenNetwork; + USHORT MobileCountryCode; + USHORT MobileNetworkCode; +} QMINAS_SET_FORBIDDEN_NETWORK_REQ_MSG, *PQMINAS_SET_FORBIDDEN_NETWORK_REQ_MSG; + +typedef struct _QMINAS_SET_FORBIDDEN_NETWORK_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_SET_FORBIDDEN_NETWORK_RESP_MSG, *PQMINAS_SET_FORBIDDEN_NETWORK_RESP_MSG; + +typedef struct _QMINAS_PERFORM_NETWORK_SCAN_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMINAS_PERFORM_NETWORK_SCAN_REQ_MSG, *PQMINAS_PERFORM_NETWORK_SCAN_REQ_MSG; + +typedef struct _VISIBLE_NETWORK +{ + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + UCHAR NetworkStatus; + UCHAR NetworkDesclen; +} VISIBLE_NETWORK, *PVISIBLE_NETWORK; + +typedef struct _QMINAS_PERFORM_NETWORK_SCAN_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_PERFORM_NETWORK_SCAN_RESP_MSG, *PQMINAS_PERFORM_NETWORK_SCAN_RESP_MSG; + +typedef struct _QMINAS_PERFORM_NETWORK_SCAN_NETWORK_INFO +{ + UCHAR TLVType; // 0x010 - required parameter + USHORT TLVLength; // length + USHORT NumNetworkInstances; +} QMINAS_PERFORM_NETWORK_SCAN_NETWORK_INFO, *PQMINAS_PERFORM_NETWORK_SCAN_NETWORK_INFO; + +typedef struct _QMINAS_PERFORM_NETWORK_SCAN_RAT_INFO +{ + UCHAR TLVType; // 0x011 - required parameter + USHORT TLVLength; // length + USHORT NumInst; +} QMINAS_PERFORM_NETWORK_SCAN_RAT_INFO, *PQMINAS_PERFORM_NETWORK_SCAN_RAT_INFO; + +typedef struct _QMINAS_PERFORM_NETWORK_SCAN_RAT +{ + USHORT MCC; + USHORT MNC; + UCHAR RAT; +} QMINAS_PERFORM_NETWORK_SCAN_RAT, *PQMINAS_PERFORM_NETWORK_SCAN_RAT; + + +typedef struct _QMINAS_MANUAL_NW_REGISTER +{ + UCHAR TLV2Type; // 0x02 - result code + USHORT TLV2Length; // 4 + USHORT MobileCountryCode; + USHORT MobileNetworkCode; + UCHAR RadioAccess; +} QMINAS_MANUAL_NW_REGISTER, *PQMINAS_MANUAL_NW_REGISTER; + +typedef struct _QMINAS_INITIATE_NW_REGISTER_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + UCHAR RegisterAction; +} QMINAS_INITIATE_NW_REGISTER_REQ_MSG, *PQMINAS_INITIATE_NW_REGISTER_REQ_MSG; + +typedef struct _QMINAS_INITIATE_NW_REGISTER_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_INITIATE_NW_REGISTER_RESP_MSG, *PQMINAS_INITIATE_NW_REGISTER_RESP_MSG; + +typedef struct _QMINAS_SET_TECHNOLOGY_PREF_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT TechPref; + UCHAR Duration; +} QMINAS_SET_TECHNOLOGY_PREF_REQ_MSG, *PQMINAS_SET_TECHNOLOGY_PREF_REQ_MSG; + +typedef struct _QMINAS_SET_TECHNOLOGY_PREF_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_SET_TECHNOLOGY_PREF_RESP_MSG, *PQMINAS_SET_TECHNOLOGY_PREF_RESP_MSG; + +typedef struct _QMINAS_GET_SIGNAL_STRENGTH_REQ_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; +} QMINAS_GET_SIGNAL_STRENGTH_REQ_MSG, *PQMINAS_GET_SIGNAL_STRENGTH_REQ_MSG; + +typedef struct _QMINAS_SIGNAL_STRENGTH +{ + CHAR SigStrength; + UCHAR RadioIf; +} QMINAS_SIGNAL_STRENGTH, *PQMINAS_SIGNAL_STRENGTH; + +typedef struct _QMINAS_SIGNAL_STRENGTH_LIST +{ + UCHAR TLV3Type; + USHORT TLV3Length; + USHORT NumInstance; +} QMINAS_SIGNAL_STRENGTH_LIST, *PQMINAS_SIGNAL_STRENGTH_LIST; + + +typedef struct _QMINAS_GET_SIGNAL_STRENGTH_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; + USHORT TLV2Length; + CHAR SignalStrength; + UCHAR RadioIf; +} QMINAS_GET_SIGNAL_STRENGTH_RESP_MSG, *PQMINAS_GET_SIGNAL_STRENGTH_RESP_MSG; + + +typedef struct _QMINAS_SET_EVENT_REPORT_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR ReportSigStrength; + UCHAR NumTresholds; + CHAR TresholdList[2]; +} QMINAS_SET_EVENT_REPORT_REQ_MSG, *PQMINAS_SET_EVENT_REPORT_REQ_MSG; + +typedef struct _QMINAS_SET_EVENT_REPORT_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_SET_EVENT_REPORT_RESP_MSG, *PQMINAS_SET_EVENT_REPORT_RESP_MSG; + +typedef struct _QMINAS_SIGNAL_STRENGTH_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + CHAR SigStrength; + UCHAR RadioIf; +} QMINAS_SIGNAL_STRENGTH_TLV, *PQMINAS_SIGNAL_STRENGTH_TLV; + +typedef struct _QMINAS_REJECT_CAUSE_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR ServiceDomain; + USHORT RejectCause; +} QMINAS_REJECT_CAUSE_TLV, *PQMINAS_REJECT_CAUSE_TLV; + +typedef struct _QMINAS_EVENT_REPORT_IND_MSG +{ + USHORT Type; + USHORT Length; +} QMINAS_EVENT_REPORT_IND_MSG, *PQMINAS_EVENT_REPORT_IND_MSG; + +typedef struct _QMINAS_GET_RF_BAND_INFO_REQ_MSG +{ + USHORT Type; + USHORT Length; +} QMINAS_GET_RF_BAND_INFO_REQ_MSG, *PQMINAS_GET_RF_BAND_INFO_REQ_MSG; + +typedef struct _QMINASRF_BAND_INFO +{ + UCHAR RadioIf; + USHORT ActiveBand; + USHORT ActiveChannel; +} QMINASRF_BAND_INFO, *PQMINASRF_BAND_INFO; + +typedef struct _QMINAS_GET_RF_BAND_INFO_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR NumInstances; +} QMINAS_GET_RF_BAND_INFO_RESP_MSG, *PQMINAS_GET_RF_BAND_INFO_RESP_MSG; + + +typedef struct _QMINAS_GET_PLMN_NAME_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT MCC; + USHORT MNC; +} QMINAS_GET_PLMN_NAME_REQ_MSG, *PQMINAS_GET_PLMN_NAME_REQ_MSG; + +typedef struct _QMINAS_GET_PLMN_NAME_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_GET_PLMN_NAME_RESP_MSG, *PQMINAS_GET_PLMN_NAME_RESP_MSG; + +typedef struct _QMINAS_GET_PLMN_NAME_SPN +{ + UCHAR TLVType; + USHORT TLVLength; + UCHAR SPN_Enc; + UCHAR SPN_Len; +} QMINAS_GET_PLMN_NAME_SPN, *PQMINAS_GET_PLMN_NAME_SPN; + +typedef struct _QMINAS_GET_PLMN_NAME_PLMN +{ + UCHAR PLMN_Enc; + UCHAR PLMN_Ci; + UCHAR PLMN_SpareBits; + UCHAR PLMN_Len; +} QMINAS_GET_PLMN_NAME_PLMN, *PQMINAS_GET_PLMN_NAME_PLMN; + +typedef struct _QMINAS_INITIATE_ATTACH_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR PsAttachAction; +} QMINAS_INITIATE_ATTACH_REQ_MSG, *PQMINAS_INITIATE_ATTACH_REQ_MSG; + +typedef struct _QMINAS_INITIATE_ATTACH_RESP_MSG +{ + USHORT Type; // QMUX type 0x0003 + USHORT Length; + UCHAR TLVType; // 0x02 - result code + USHORT TLVLength; // 4 + USHORT QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + USHORT QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} QMINAS_INITIATE_ATTACH_RESP_MSG, *PQMINAS_INITIATE_ATTACH_RESP_MSG; +#endif +// ======================= End of NAS ============================== + +// ======================= UIM ============================== +#define QMIUIM_READ_TRANSPARENT_REQ 0x0020 +#define QMIUIM_READ_TRANSPARENT_RESP 0x0020 +#define QMIUIM_READ_TRANSPARENT_IND 0x0020 +#define QMIUIM_READ_RECORD_REQ 0x0021 +#define QMIUIM_READ_RECORD_RESP 0x0021 +#define QMIUIM_READ_RECORD_IND 0x0021 +#define QMIUIM_WRITE_TRANSPARENT_REQ 0x0022 +#define QMIUIM_WRITE_TRANSPARENT_RESP 0x0022 +#define QMIUIM_WRITE_TRANSPARENT_IND 0x0022 +#define QMIUIM_WRITE_RECORD_REQ 0x0023 +#define QMIUIM_WRITE_RECORD_RESP 0x0023 +#define QMIUIM_WRITE_RECORD_IND 0x0023 +#define QMIUIM_SET_PIN_PROTECTION_REQ 0x0025 +#define QMIUIM_SET_PIN_PROTECTION_RESP 0x0025 +#define QMIUIM_SET_PIN_PROTECTION_IND 0x0025 +#define QMIUIM_VERIFY_PIN_REQ 0x0026 +#define QMIUIM_VERIFY_PIN_RESP 0x0026 +#define QMIUIM_VERIFY_PIN_IND 0x0026 +#define QMIUIM_UNBLOCK_PIN_REQ 0x0027 +#define QMIUIM_UNBLOCK_PIN_RESP 0x0027 +#define QMIUIM_UNBLOCK_PIN_IND 0x0027 +#define QMIUIM_CHANGE_PIN_REQ 0x0028 +#define QMIUIM_CHANGE_PIN_RESP 0x0028 +#define QMIUIM_CHANGE_PIN_IND 0x0028 +#define QMIUIM_DEPERSONALIZATION_REQ 0x0029 +#define QMIUIM_DEPERSONALIZATION_RESP 0x0029 +#define QMIUIM_EVENT_REG_REQ 0x002E +#define QMIUIM_EVENT_REG_RESP 0x002E +#define QMIUIM_GET_CARD_STATUS_REQ 0x002F +#define QMIUIM_GET_CARD_STATUS_RESP 0x002F +#define QMIUIM_STATUS_CHANGE_IND 0x0032 + + +typedef struct _QMIUIM_GET_CARD_STATUS_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} __attribute__ ((packed)) QMIUIM_GET_CARD_STATUS_RESP_MSG, *PQMIUIM_GET_CARD_STATUS_RESP_MSG; + +typedef struct _QMIUIM_CARD_STATUS +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT IndexGWPri; + USHORT Index1XPri; + USHORT IndexGWSec; + USHORT Index1XSec; + UCHAR NumSlot; + UCHAR CardState; + UCHAR UPINState; + UCHAR UPINRetries; + UCHAR UPUKRetries; + UCHAR ErrorCode; + UCHAR NumApp; + UCHAR AppType; + UCHAR AppState; + UCHAR PersoState; + UCHAR PersoFeature; + UCHAR PersoRetries; + UCHAR PersoUnblockRetries; + UCHAR AIDLength; +} __attribute__ ((packed)) QMIUIM_CARD_STATUS, *PQMIUIM_CARD_STATUS; + +typedef struct _QMIUIM_PIN_STATE +{ + UCHAR UnivPIN; + UCHAR PIN1State; + UCHAR PIN1Retries; + UCHAR PUK1Retries; + UCHAR PIN2State; + UCHAR PIN2Retries; + UCHAR PUK2Retries; +} __attribute__ ((packed)) QMIUIM_PIN_STATE, *PQMIUIM_PIN_STATE; + +typedef struct _QMIUIM_VERIFY_PIN_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR Session_Type; + UCHAR Aid_Len; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR PINID; + UCHAR PINLen; + UCHAR PINValue; +} __attribute__ ((packed)) QMIUIM_VERIFY_PIN_REQ_MSG, *PQMIUIM_VERIFY_PIN_REQ_MSG; + +typedef struct _QMIUIM_VERIFY_PIN_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; + UCHAR TLV2Type; + USHORT TLV2Length; + UCHAR PINVerifyRetriesLeft; + UCHAR PINUnblockRetriesLeft; +} __attribute__ ((packed)) QMIUIM_VERIFY_PIN_RESP_MSG, *PQMIUIM_VERIFY_PIN_RESP_MSG; + +typedef struct _QMIUIM_READ_TRANSPARENT_REQ_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + UCHAR Session_Type; + UCHAR Aid_Len; + UCHAR TLV2Type; + USHORT TLV2Length; + USHORT file_id; + UCHAR path_len; + UCHAR path[]; +} __attribute__ ((packed)) QMIUIM_READ_TRANSPARENT_REQ_MSG, *PQMIUIM_READ_TRANSPARENT_REQ_MSG; + +typedef struct _READ_TRANSPARENT_TLV +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT Offset; + USHORT Length; +} __attribute__ ((packed)) READ_TRANSPARENT_TLV, *PREAD_TRANSPARENT_TLV; + +typedef struct _QMIUIM_CONTENT +{ + UCHAR TLVType; + USHORT TLVLength; + USHORT content_len; + UCHAR content[]; +} __attribute__ ((packed)) QMIUIM_CONTENT, *PQMIUIM_CONTENT; + +typedef struct _QMIUIM_READ_TRANSPARENT_RESP_MSG +{ + USHORT Type; + USHORT Length; + UCHAR TLVType; + USHORT TLVLength; + USHORT QMUXResult; + USHORT QMUXError; +} __attribute__ ((packed)) QMIUIM_READ_TRANSPARENT_RESP_MSG, *PQMIUIM_READ_TRANSPARENT_RESP_MSG; + +typedef struct _QMUX_MSG +{ + QCQMUX_HDR QMUXHdr; + union + { + // Message Header + QCQMUX_MSG_HDR QMUXMsgHdr; + QCQMUX_MSG_HDR_RESP QMUXMsgHdrResp; + + // QMIWDS Message +#if 0 + QMIWDS_GET_PKT_SRVC_STATUS_REQ_MSG PacketServiceStatusReq; + QMIWDS_GET_PKT_SRVC_STATUS_RESP_MSG PacketServiceStatusRsp; + QMIWDS_GET_PKT_SRVC_STATUS_IND_MSG PacketServiceStatusInd; + QMIWDS_EVENT_REPORT_IND_MSG EventReportInd; + QMIWDS_GET_CURRENT_CHANNEL_RATE_REQ_MSG GetCurrChannelRateReq; + QMIWDS_GET_CURRENT_CHANNEL_RATE_RESP_MSG GetCurrChannelRateRsp; + QMIWDS_GET_PKT_STATISTICS_REQ_MSG GetPktStatsReq; + QMIWDS_GET_PKT_STATISTICS_RESP_MSG GetPktStatsRsp; + QMIWDS_SET_EVENT_REPORT_REQ_MSG EventReportReq; + QMIWDS_SET_EVENT_REPORT_RESP_MSG EventReportRsp; +#endif + //#ifdef QC_IP_MODE + QMIWDS_GET_RUNTIME_SETTINGS_REQ_MSG GetRuntimeSettingsReq; + QMIWDS_GET_RUNTIME_SETTINGS_RESP_MSG GetRuntimeSettingsRsp; + //#endif // QC_IP_MODE + QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ_MSG SetClientIpFamilyPrefReq; + QMIWDS_SET_CLIENT_IP_FAMILY_PREF_RESP_MSG SetClientIpFamilyPrefResp; + QMIWDS_SET_AUTO_CONNECT_REQ_MSG SetAutoConnectReq; +#if 0 + QMIWDS_GET_MIP_MODE_REQ_MSG GetMipModeReq; + QMIWDS_GET_MIP_MODE_RESP_MSG GetMipModeResp; +#endif + QMIWDS_START_NETWORK_INTERFACE_REQ_MSG StartNwInterfaceReq; + QMIWDS_START_NETWORK_INTERFACE_RESP_MSG StartNwInterfaceResp; + QMIWDS_STOP_NETWORK_INTERFACE_REQ_MSG StopNwInterfaceReq; + QMIWDS_STOP_NETWORK_INTERFACE_RESP_MSG StopNwInterfaceResp; + QMIWDS_GET_DEFAULT_SETTINGS_REQ_MSG GetDefaultSettingsReq; + QMIWDS_GET_DEFAULT_SETTINGS_RESP_MSG GetDefaultSettingsResp; + QMIWDS_MODIFY_PROFILE_SETTINGS_REQ_MSG ModifyProfileSettingsReq; + QMIWDS_MODIFY_PROFILE_SETTINGS_RESP_MSG ModifyProfileSettingsResp; + QMIWDS_GET_PROFILE_SETTINGS_REQ_MSG GetProfileSettingsReq; +#if 0 + QMIWDS_GET_DATA_BEARER_REQ_MSG GetDataBearerReq; + QMIWDS_GET_DATA_BEARER_RESP_MSG GetDataBearerResp; + QMIWDS_DUN_CALL_INFO_REQ_MSG DunCallInfoReq; + QMIWDS_DUN_CALL_INFO_RESP_MSG DunCallInfoResp; +#endif + QMIWDS_BIND_MUX_DATA_PORT_REQ_MSG BindMuxDataPortReq; + + // QMIDMS Messages +#if 0 + QMIDMS_GET_DEVICE_MFR_REQ_MSG GetDeviceMfrReq; + QMIDMS_GET_DEVICE_MFR_RESP_MSG GetDeviceMfrRsp; + QMIDMS_GET_DEVICE_MODEL_ID_REQ_MSG GetDeviceModeIdReq; + QMIDMS_GET_DEVICE_MODEL_ID_RESP_MSG GetDeviceModeIdRsp; + QMIDMS_GET_DEVICE_REV_ID_REQ_MSG GetDeviceRevIdReq; + QMIDMS_GET_DEVICE_REV_ID_RESP_MSG GetDeviceRevIdRsp; + QMIDMS_GET_MSISDN_REQ_MSG GetMsisdnReq; + QMIDMS_GET_MSISDN_RESP_MSG GetMsisdnRsp; + QMIDMS_GET_DEVICE_SERIAL_NUMBERS_REQ_MSG GetDeviceSerialNumReq; + QMIDMS_GET_DEVICE_SERIAL_NUMBERS_RESP_MSG GetDeviceSerialNumRsp; + QMIDMS_GET_DEVICE_CAP_REQ_MSG GetDeviceCapReq; + QMIDMS_GET_DEVICE_CAP_RESP_MSG GetDeviceCapResp; + QMIDMS_GET_BAND_CAP_REQ_MSG GetBandCapReq; + QMIDMS_GET_BAND_CAP_RESP_MSG GetBandCapRsp; + QMIDMS_GET_ACTIVATED_STATUS_REQ_MSG GetActivatedStatusReq; + QMIDMS_GET_ACTIVATED_STATUS_RESP_MSG GetActivatedStatusResp; + QMIDMS_GET_OPERATING_MODE_REQ_MSG GetOperatingModeReq; + QMIDMS_GET_OPERATING_MODE_RESP_MSG GetOperatingModeResp; +#endif + QMIDMS_SET_OPERATING_MODE_REQ_MSG SetOperatingModeReq; + QMIDMS_SET_OPERATING_MODE_RESP_MSG SetOperatingModeResp; +#if 0 + QMIDMS_UIM_GET_ICCID_REQ_MSG GetICCIDReq; + QMIDMS_UIM_GET_ICCID_RESP_MSG GetICCIDResp; + QMIDMS_ACTIVATE_AUTOMATIC_REQ_MSG ActivateAutomaticReq; + QMIDMS_ACTIVATE_AUTOMATIC_RESP_MSG ActivateAutomaticResp; + QMIDMS_ACTIVATE_MANUAL_REQ_MSG ActivateManualReq; + QMIDMS_ACTIVATE_MANUAL_RESP_MSG ActivateManualResp; +#endif + QMIDMS_UIM_GET_PIN_STATUS_REQ_MSG UIMGetPinStatusReq; + QMIDMS_UIM_GET_PIN_STATUS_RESP_MSG UIMGetPinStatusResp; + QMIDMS_UIM_VERIFY_PIN_REQ_MSG UIMVerifyPinReq; + QMIDMS_UIM_VERIFY_PIN_RESP_MSG UIMVerifyPinResp; +#if 0 + QMIDMS_UIM_SET_PIN_PROTECTION_REQ_MSG UIMSetPinProtectionReq; + QMIDMS_UIM_SET_PIN_PROTECTION_RESP_MSG UIMSetPinProtectionResp; + QMIDMS_UIM_CHANGE_PIN_REQ_MSG UIMChangePinReq; + QMIDMS_UIM_CHANGE_PIN_RESP_MSG UIMChangePinResp; + QMIDMS_UIM_UNBLOCK_PIN_REQ_MSG UIMUnblockPinReq; + QMIDMS_UIM_UNBLOCK_PIN_RESP_MSG UIMUnblockPinResp; + QMIDMS_SET_EVENT_REPORT_REQ_MSG DmsSetEventReportReq; + QMIDMS_SET_EVENT_REPORT_RESP_MSG DmsSetEventReportResp; + QMIDMS_EVENT_REPORT_IND_MSG DmsEventReportInd; +#endif + QMIDMS_UIM_GET_STATE_REQ_MSG UIMGetStateReq; + QMIDMS_UIM_GET_STATE_RESP_MSG UIMGetStateResp; + QMIDMS_UIM_GET_IMSI_REQ_MSG UIMGetIMSIReq; + QMIDMS_UIM_GET_IMSI_RESP_MSG UIMGetIMSIResp; +#if 0 + QMIDMS_UIM_GET_CK_STATUS_REQ_MSG UIMGetCkStatusReq; + QMIDMS_UIM_GET_CK_STATUS_RESP_MSG UIMGetCkStatusResp; + QMIDMS_UIM_SET_CK_PROTECTION_REQ_MSG UIMSetCkProtectionReq; + QMIDMS_UIM_SET_CK_PROTECTION_RESP_MSG UIMSetCkProtectionResp; + QMIDMS_UIM_UNBLOCK_CK_REQ_MSG UIMUnblockCkReq; + QMIDMS_UIM_UNBLOCK_CK_RESP_MSG UIMUnblockCkResp; +#endif + + // QMIQOS Messages +#if 0 + QMI_QOS_SET_EVENT_REPORT_REQ_MSG QosSetEventReportReq; + QMI_QOS_SET_EVENT_REPORT_RESP_MSG QosSetEventReportRsp; + QMI_QOS_EVENT_REPORT_IND_MSG QosEventReportInd; +#endif + + // QMIWMS Messages +#if 0 + QMIWMS_GET_MESSAGE_PROTOCOL_REQ_MSG GetMessageProtocolReq; + QMIWMS_GET_MESSAGE_PROTOCOL_RESP_MSG GetMessageProtocolResp; + QMIWMS_GET_SMSC_ADDRESS_REQ_MSG GetSMSCAddressReq; + QMIWMS_GET_SMSC_ADDRESS_RESP_MSG GetSMSCAddressResp; + QMIWMS_SET_SMSC_ADDRESS_REQ_MSG SetSMSCAddressReq; + QMIWMS_SET_SMSC_ADDRESS_RESP_MSG SetSMSCAddressResp; + QMIWMS_GET_STORE_MAX_SIZE_REQ_MSG GetStoreMaxSizeReq; + QMIWMS_GET_STORE_MAX_SIZE_RESP_MSG GetStoreMaxSizeResp; + QMIWMS_LIST_MESSAGES_REQ_MSG ListMessagesReq; + QMIWMS_LIST_MESSAGES_RESP_MSG ListMessagesResp; + QMIWMS_RAW_READ_REQ_MSG RawReadMessagesReq; + QMIWMS_RAW_READ_RESP_MSG RawReadMessagesResp; + QMIWMS_SET_EVENT_REPORT_REQ_MSG WmsSetEventReportReq; + QMIWMS_SET_EVENT_REPORT_RESP_MSG WmsSetEventReportResp; + QMIWMS_EVENT_REPORT_IND_MSG WmsEventReportInd; + QMIWMS_DELETE_REQ_MSG WmsDeleteReq; + QMIWMS_DELETE_RESP_MSG WmsDeleteResp; + QMIWMS_RAW_SEND_REQ_MSG RawSendMessagesReq; + QMIWMS_RAW_SEND_RESP_MSG RawSendMessagesResp; + QMIWMS_MODIFY_TAG_REQ_MSG WmsModifyTagReq; + QMIWMS_MODIFY_TAG_RESP_MSG WmsModifyTagResp; +#endif + + // QMINAS Messages +#if 0 + QMINAS_GET_HOME_NETWORK_REQ_MSG GetHomeNetworkReq; + QMINAS_GET_HOME_NETWORK_RESP_MSG GetHomeNetworkResp; + QMINAS_GET_PREFERRED_NETWORK_REQ_MSG GetPreferredNetworkReq; + QMINAS_GET_PREFERRED_NETWORK_RESP_MSG GetPreferredNetworkResp; + QMINAS_GET_FORBIDDEN_NETWORK_REQ_MSG GetForbiddenNetworkReq; + QMINAS_GET_FORBIDDEN_NETWORK_RESP_MSG GetForbiddenNetworkResp; + QMINAS_GET_SERVING_SYSTEM_REQ_MSG GetServingSystemReq; +#endif + QMINAS_GET_SERVING_SYSTEM_RESP_MSG GetServingSystemResp; + QMINAS_GET_SYS_INFO_RESP_MSG GetSysInfoResp; + QMINAS_SYS_INFO_IND_MSG NasSysInfoInd; +#if 0 + QMINAS_SERVING_SYSTEM_IND_MSG NasServingSystemInd; + QMINAS_SET_PREFERRED_NETWORK_REQ_MSG SetPreferredNetworkReq; + QMINAS_SET_PREFERRED_NETWORK_RESP_MSG SetPreferredNetworkResp; + QMINAS_SET_FORBIDDEN_NETWORK_REQ_MSG SetForbiddenNetworkReq; + QMINAS_SET_FORBIDDEN_NETWORK_RESP_MSG SetForbiddenNetworkResp; + QMINAS_PERFORM_NETWORK_SCAN_REQ_MSG PerformNetworkScanReq; + QMINAS_PERFORM_NETWORK_SCAN_RESP_MSG PerformNetworkScanResp; + QMINAS_INITIATE_NW_REGISTER_REQ_MSG InitiateNwRegisterReq; + QMINAS_INITIATE_NW_REGISTER_RESP_MSG InitiateNwRegisterResp; + QMINAS_SET_TECHNOLOGY_PREF_REQ_MSG SetTechnologyPrefReq; + QMINAS_SET_TECHNOLOGY_PREF_RESP_MSG SetTechnologyPrefResp; + QMINAS_GET_SIGNAL_STRENGTH_REQ_MSG GetSignalStrengthReq; + QMINAS_GET_SIGNAL_STRENGTH_RESP_MSG GetSignalStrengthResp; + QMINAS_SET_EVENT_REPORT_REQ_MSG SetEventReportReq; + QMINAS_SET_EVENT_REPORT_RESP_MSG SetEventReportResp; + QMINAS_EVENT_REPORT_IND_MSG NasEventReportInd; + QMINAS_GET_RF_BAND_INFO_REQ_MSG GetRFBandInfoReq; + QMINAS_GET_RF_BAND_INFO_RESP_MSG GetRFBandInfoResp; + QMINAS_INITIATE_ATTACH_REQ_MSG InitiateAttachReq; + QMINAS_INITIATE_ATTACH_RESP_MSG InitiateAttachResp; + QMINAS_GET_PLMN_NAME_REQ_MSG GetPLMNNameReq; + QMINAS_GET_PLMN_NAME_RESP_MSG GetPLMNNameResp; +#endif + + // QMIUIM Messages + QMIUIM_GET_CARD_STATUS_RESP_MSG UIMGetCardStatus; + QMIUIM_VERIFY_PIN_REQ_MSG UIMUIMVerifyPinReq; + QMIUIM_VERIFY_PIN_RESP_MSG UIMUIMVerifyPinResp; +#if 0 + QMIUIM_SET_PIN_PROTECTION_REQ_MSG UIMUIMSetPinProtectionReq; + QMIUIM_SET_PIN_PROTECTION_RESP_MSG UIMUIMSetPinProtectionResp; + QMIUIM_CHANGE_PIN_REQ_MSG UIMUIMChangePinReq; + QMIUIM_CHANGE_PIN_RESP_MSG UIMUIMChangePinResp; + QMIUIM_UNBLOCK_PIN_REQ_MSG UIMUIMUnblockPinReq; + QMIUIM_UNBLOCK_PIN_RESP_MSG UIMUIMUnblockPinResp; +#endif + QMIUIM_READ_TRANSPARENT_REQ_MSG UIMUIMReadTransparentReq; + QMIUIM_READ_TRANSPARENT_RESP_MSG UIMUIMReadTransparentResp; + + QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq; + + }; +} __attribute__ ((packed)) QMUX_MSG, *PQMUX_MSG; + +#pragma pack(pop) + +#endif // MPQMUX_H diff --git a/quectel-CM/Makefile b/quectel-CM/Makefile new file mode 100755 index 0000000..9bbb0d7 --- /dev/null +++ b/quectel-CM/Makefile @@ -0,0 +1,12 @@ +release: clean qmi-proxy + $(CC) -Wall -s QmiWwanCM.c GobiNetCM.c main.c MPQMUX.c QMIThread.c util.c udhcpc.c -o quectel-CM -lpthread -ldl + +debug: clean + $(CC) -Wall -g QmiWwanCM.c GobiNetCM.c main.c MPQMUX.c QMIThread.c util.c udhcpc.c -o quectel-CM -lpthread -ldl + +qmi-proxy: + $(CC) -Wall -s quectel-qmi-proxy.c -o quectel-qmi-proxy -lpthread -ldl + +clean: + rm -rf quectel-CM *~ + rm -rf quectel-qmi-proxy diff --git a/quectel-CM/QMIThread.c b/quectel-CM/QMIThread.c new file mode 100755 index 0000000..e66505c --- /dev/null +++ b/quectel-CM/QMIThread.c @@ -0,0 +1,1927 @@ +#include "QMIThread.h" +extern char *strndup (const char *__string, size_t __n); + +#define qmi_rsp_check_and_return() do { \ + if (err < 0 || pResponse == NULL) { \ + dbg_time("%s err = %d", __func__, err); \ + return err; \ + } \ + pMUXMsg = &pResponse->MUXMsg; \ + if (le16_to_cpu(pMUXMsg->QMUXMsgHdrResp.QMUXResult) || le16_to_cpu(pMUXMsg->QMUXMsgHdrResp.QMUXError)) { \ + USHORT QMUXError = le16_to_cpu(pMUXMsg->QMUXMsgHdrResp.QMUXError); \ + dbg_time("%s QMUXResult = 0x%x, QMUXError = 0x%x", __func__, \ + le16_to_cpu(pMUXMsg->QMUXMsgHdrResp.QMUXResult), QMUXError); \ + free(pResponse); \ + return QMUXError; \ + } \ +} while(0) + +int qmiclientId[QMUX_TYPE_WDS_ADMIN + 1]; //GobiNet use fd to indicate client ID, so type of qmiclientId must be int +static uint32_t WdsConnectionIPv4Handle = 0; +static uint32_t WdsConnectionIPv6Handle = 0; +static int s_is_cdma = 0; +static int s_hdr_personality = 0; // 0x01-HRPD, 0x02-eHRPD +static char *qstrcpy(char *to, const char *from) { //no __strcpy_chk + char *save = to; + for (; (*to = *from) != '\0'; ++from, ++to); + return(save); +} + +static int s_9x07 = -1; + +typedef USHORT (*CUSTOMQMUX)(PQMUX_MSG pMUXMsg, void *arg); + +// To retrieve the ith (Index) TLV +PQMI_TLV_HDR GetTLV (PQCQMUX_MSG_HDR pQMUXMsgHdr, int TLVType) { + int TLVFind = 0; + USHORT Length = le16_to_cpu(pQMUXMsgHdr->Length); + PQMI_TLV_HDR pTLVHdr = (PQMI_TLV_HDR)(pQMUXMsgHdr + 1); + + while (Length >= sizeof(QMI_TLV_HDR)) { + TLVFind++; + if (TLVType > 0x1000) { + if ((TLVFind + 0x1000) == TLVType) + return pTLVHdr; + } else if (pTLVHdr->TLVType == TLVType) { + return pTLVHdr; + } + + Length -= (le16_to_cpu((pTLVHdr->TLVLength)) + sizeof(QMI_TLV_HDR)); + pTLVHdr = (PQMI_TLV_HDR)(((UCHAR *)pTLVHdr) + le16_to_cpu(pTLVHdr->TLVLength) + sizeof(QMI_TLV_HDR)); + } + + return NULL; +} + +static USHORT GetQMUXTransactionId(void) { + static int TransactionId = 0; + if (++TransactionId > 0xFFFF) + TransactionId = 1; + return TransactionId; +} + +static PQCQMIMSG ComposeQMUXMsg(UCHAR QMIType, USHORT Type, CUSTOMQMUX customQmuxMsgFunction, void *arg) { + UCHAR QMIBuf[WDM_DEFAULT_BUFSIZE]; + PQCQMIMSG pRequest = (PQCQMIMSG)QMIBuf; + int Length; + + memset(QMIBuf, 0x00, sizeof(QMIBuf)); + pRequest->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; + pRequest->QMIHdr.CtlFlags = 0x00; + pRequest->QMIHdr.QMIType = QMIType; + pRequest->QMIHdr.ClientId = qmiclientId[QMIType] & 0xFF; + + if (qmiclientId[QMIType] == 0) { + dbg_time("QMIType %d has no clientID", QMIType); + return NULL; + } + + pRequest->MUXMsg.QMUXHdr.CtlFlags = QMUX_CTL_FLAG_SINGLE_MSG | QMUX_CTL_FLAG_TYPE_CMD; + pRequest->MUXMsg.QMUXHdr.TransactionId = cpu_to_le16(GetQMUXTransactionId()); + pRequest->MUXMsg.QMUXMsgHdr.Type = cpu_to_le16(Type); + if (customQmuxMsgFunction) + pRequest->MUXMsg.QMUXMsgHdr.Length = cpu_to_le16(customQmuxMsgFunction(&pRequest->MUXMsg, arg) - sizeof(QCQMUX_MSG_HDR)); + else + pRequest->MUXMsg.QMUXMsgHdr.Length = cpu_to_le16(0x0000); + + pRequest->QMIHdr.Length = cpu_to_le16(le16_to_cpu(pRequest->MUXMsg.QMUXMsgHdr.Length) + sizeof(QCQMUX_MSG_HDR) + sizeof(QCQMUX_HDR) + + sizeof(QCQMI_HDR) - 1); + Length = le16_to_cpu(pRequest->QMIHdr.Length) + 1; + + pRequest = (PQCQMIMSG)malloc(Length); + if (pRequest == NULL) { + dbg_time("%s fail to malloc", __func__); + } else { + memcpy(pRequest, QMIBuf, Length); + } + + return pRequest; +} + +#if 0 +static USHORT NasSetEventReportReq(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->SetEventReportReq.TLVType = 0x10; + pMUXMsg->SetEventReportReq.TLVLength = 0x04; + pMUXMsg->SetEventReportReq.ReportSigStrength = 0x00; + pMUXMsg->SetEventReportReq.NumTresholds = 2; + pMUXMsg->SetEventReportReq.TresholdList[0] = -113; + pMUXMsg->SetEventReportReq.TresholdList[1] = -50; + return sizeof(QMINAS_SET_EVENT_REPORT_REQ_MSG); +} + +static USHORT WdsSetEventReportReq(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->EventReportReq.TLVType = 0x10; // 0x10 -- current channel rate indicator + pMUXMsg->EventReportReq.TLVLength = 0x0001; // 1 + pMUXMsg->EventReportReq.Mode = 0x00; // 0-do not report; 1-report when rate changes + + pMUXMsg->EventReportReq.TLV2Type = 0x11; // 0x11 + pMUXMsg->EventReportReq.TLV2Length = 0x0005; // 5 + pMUXMsg->EventReportReq.StatsPeriod = 0x00; // seconds between reports; 0-do not report + pMUXMsg->EventReportReq.StatsMask = 0x000000ff; // + + pMUXMsg->EventReportReq.TLV3Type = 0x12; // 0x12 -- current data bearer indicator + pMUXMsg->EventReportReq.TLV3Length = 0x0001; // 1 + pMUXMsg->EventReportReq.Mode3 = 0x01; // 0-do not report; 1-report when changes + + pMUXMsg->EventReportReq.TLV4Type = 0x13; // 0x13 -- dormancy status indicator + pMUXMsg->EventReportReq.TLV4Length = 0x0001; // 1 + pMUXMsg->EventReportReq.DormancyStatus = 0x00; // 0-do not report; 1-report when changes + return sizeof(QMIWDS_SET_EVENT_REPORT_REQ_MSG); +} + +static USHORT DmsSetEventReportReq(PQMUX_MSG pMUXMsg) { + PPIN_STATUS pPinState = (PPIN_STATUS)(&pMUXMsg->DmsSetEventReportReq + 1); + PUIM_STATE pUimState = (PUIM_STATE)(pPinState + 1); + // Pin State + pPinState->TLVType = 0x12; + pPinState->TLVLength = 0x01; + pPinState->ReportPinState = 0x01; + // UIM State + pUimState->TLVType = 0x15; + pUimState->TLVLength = 0x01; + pUimState->UIMState = 0x01; + return sizeof(QMIDMS_SET_EVENT_REPORT_REQ_MSG) + sizeof(PIN_STATUS) + sizeof(UIM_STATE); +} +#endif + +static USHORT WdsStartNwInterfaceReq(PQMUX_MSG pMUXMsg, void *arg) { + PQMIWDS_TECHNOLOGY_PREFERECE pTechPref; + PQMIWDS_AUTH_PREFERENCE pAuthPref; + PQMIWDS_USERNAME pUserName; + PQMIWDS_PASSWD pPasswd; + PQMIWDS_APNNAME pApnName; + PQMIWDS_IP_FAMILY_TLV pIpFamily; + USHORT TLVLength = 0; + UCHAR *pTLV; + PROFILE_T *profile = (PROFILE_T *)arg; + const char *profile_user = profile->user; + const char *profile_password = profile->password; + int profile_auth = profile->auth; + + if (s_is_cdma && (profile_user == NULL || profile_user[0] == '\0') && (profile_password == NULL || profile_password[0] == '\0')) { + profile_user = "ctnet@mycdma.cn"; + profile_password = "vnet.mobi"; + profile_auth = 2; //chap + } + + pTLV = (UCHAR *)(&pMUXMsg->StartNwInterfaceReq + 1); + pMUXMsg->StartNwInterfaceReq.Length = 0; + + // Set technology Preferece + pTechPref = (PQMIWDS_TECHNOLOGY_PREFERECE)(pTLV + TLVLength); + pTechPref->TLVType = 0x30; + pTechPref->TLVLength = cpu_to_le16(0x01); + if (s_is_cdma == 0) + pTechPref->TechPreference = 0x01; + else + pTechPref->TechPreference = 0x02; + TLVLength +=(le16_to_cpu(pTechPref->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + + // Set APN Name + if (profile->apn && !s_is_cdma) { //cdma no apn + pApnName = (PQMIWDS_APNNAME)(pTLV + TLVLength); + pApnName->TLVType = 0x14; + pApnName->TLVLength = cpu_to_le16(strlen(profile->apn)); + qstrcpy((char *)&pApnName->ApnName, profile->apn); + TLVLength +=(le16_to_cpu(pApnName->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Set User Name + if (profile_user) { + pUserName = (PQMIWDS_USERNAME)(pTLV + TLVLength); + pUserName->TLVType = 0x17; + pUserName->TLVLength = cpu_to_le16(strlen(profile_user)); + qstrcpy((char *)&pUserName->UserName, profile_user); + TLVLength += (le16_to_cpu(pUserName->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Set Password + if (profile_password) { + pPasswd = (PQMIWDS_PASSWD)(pTLV + TLVLength); + pPasswd->TLVType = 0x18; + pPasswd->TLVLength = cpu_to_le16(strlen(profile_password)); + qstrcpy((char *)&pPasswd->Passwd, profile_password); + TLVLength += (le16_to_cpu(pPasswd->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Set Auth Protocol + if (profile_user && profile_password) { + pAuthPref = (PQMIWDS_AUTH_PREFERENCE)(pTLV + TLVLength); + pAuthPref->TLVType = 0x16; + pAuthPref->TLVLength = cpu_to_le16(0x01); + pAuthPref->AuthPreference = profile_auth; // 0 ~ None, 1 ~ Pap, 2 ~ Chap, 3 ~ MsChapV2 + TLVLength += (le16_to_cpu(pAuthPref->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Add IP Family Preference + pIpFamily = (PQMIWDS_IP_FAMILY_TLV)(pTLV + TLVLength); + pIpFamily->TLVType = 0x19; + pIpFamily->TLVLength = cpu_to_le16(0x01); + pIpFamily->IpFamily = profile->curIpFamily; + TLVLength += (le16_to_cpu(pIpFamily->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + + //Set Profile Index + if (profile->pdp && !s_is_cdma) { //cdma only support one pdp, so no need to set profile index + PQMIWDS_PROFILE_IDENTIFIER pProfileIndex = (PQMIWDS_PROFILE_IDENTIFIER)(pTLV + TLVLength); + pProfileIndex->TLVLength = cpu_to_le16(0x01); + pProfileIndex->TLVType = 0x31; + pProfileIndex->ProfileIndex = profile->pdp; + if (s_is_cdma && s_hdr_personality == 0x02) { + pProfileIndex->TLVType = 0x32; //profile_index_3gpp2 + pProfileIndex->ProfileIndex = 101; + } + TLVLength += (le16_to_cpu(pProfileIndex->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + return sizeof(QMIWDS_START_NETWORK_INTERFACE_REQ_MSG) + TLVLength; +} + +static USHORT WdsStopNwInterfaceReq(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->StopNwInterfaceReq.TLVType = 0x01; + pMUXMsg->StopNwInterfaceReq.TLVLength = cpu_to_le16(0x04); + if (*((int *)arg) == IpFamilyV4) + pMUXMsg->StopNwInterfaceReq.Handle = cpu_to_le32(WdsConnectionIPv4Handle); + else + pMUXMsg->StopNwInterfaceReq.Handle = cpu_to_le32(WdsConnectionIPv6Handle); + return sizeof(QMIWDS_STOP_NETWORK_INTERFACE_REQ_MSG); +} + +static USHORT WdsSetClientIPFamilyPref(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->SetClientIpFamilyPrefReq.TLVType = 0x01; + pMUXMsg->SetClientIpFamilyPrefReq.TLVLength = cpu_to_le16(0x01); + pMUXMsg->SetClientIpFamilyPrefReq.IpPreference = *((UCHAR *)arg); + return sizeof(QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ_MSG); +} + +static USHORT WdsSetAutoConnect(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->SetAutoConnectReq.TLVType = 0x01; + pMUXMsg->SetAutoConnectReq.TLVLength = cpu_to_le16(0x01); + pMUXMsg->SetAutoConnectReq.autoconnect_setting = *((UCHAR *)arg); + return sizeof(QMIWDS_SET_AUTO_CONNECT_REQ_MSG); +} + +enum peripheral_ep_type { + DATA_EP_TYPE_RESERVED = 0x0, + DATA_EP_TYPE_HSIC = 0x1, + DATA_EP_TYPE_HSUSB = 0x2, + DATA_EP_TYPE_PCIE = 0x3, + DATA_EP_TYPE_EMBEDDED = 0x4, + DATA_EP_TYPE_BAM_DMUX = 0x5, +}; + +typedef struct { + UINT rx_urb_size; + enum peripheral_ep_type ep_type; + UINT iface_id; + UCHAR MuxId; +} QMAP_SETTING; +static USHORT WdsSetQMUXBindMuxDataPort(PQMUX_MSG pMUXMsg, void *arg) { + QMAP_SETTING *qmap_settings = (QMAP_SETTING *)arg; + + pMUXMsg->BindMuxDataPortReq.TLVType = 0x10; + pMUXMsg->BindMuxDataPortReq.TLVLength = cpu_to_le16(0x08); + pMUXMsg->BindMuxDataPortReq.ep_type = cpu_to_le32(qmap_settings->ep_type); + pMUXMsg->BindMuxDataPortReq.iface_id = cpu_to_le32(qmap_settings->iface_id); + pMUXMsg->BindMuxDataPortReq.TLV2Type = 0x11; + pMUXMsg->BindMuxDataPortReq.TLV2Length = cpu_to_le16(0x01); + pMUXMsg->BindMuxDataPortReq.MuxId = qmap_settings->MuxId; + pMUXMsg->BindMuxDataPortReq.TLV3Type = 0x13; + pMUXMsg->BindMuxDataPortReq.TLV3Length = cpu_to_le16(0x04); + pMUXMsg->BindMuxDataPortReq.client_type = cpu_to_le32(1); //WDS_CLIENT_TYPE_TETHERED + + return sizeof(QMIWDS_BIND_MUX_DATA_PORT_REQ_MSG); +} + +static USHORT WdaSetDataFormat(PQMUX_MSG pMUXMsg, void *arg) { + QMAP_SETTING *qmap_settings = (QMAP_SETTING *)arg; + + if (qmap_settings->rx_urb_size == 0) { + PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS pWdsAdminQosTlv; + PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV linkProto; + PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV dlTlp; + + pWdsAdminQosTlv = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS)(&pMUXMsg->QMUXMsgHdr + 1); + pWdsAdminQosTlv->TLVType = 0x10; + pWdsAdminQosTlv->TLVLength = cpu_to_le16(0x0001); + pWdsAdminQosTlv->QOSSetting = 0; /* no-QOS header */ + + linkProto = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)(pWdsAdminQosTlv + 1); + linkProto->TLVType = 0x11; + linkProto->TLVLength = cpu_to_le16(4); + linkProto->Value = cpu_to_le32(0x01); /* Set Ethernet mode */ + + dlTlp = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)(linkProto + 1);; + dlTlp->TLVType = 0x13; + dlTlp->TLVLength = cpu_to_le16(4); + dlTlp->Value = cpu_to_le32(0x00); + + if (sizeof(*linkProto) != 7 ) + dbg_time("%s sizeof(*linkProto) = %d, is not 7!", __func__, sizeof(*linkProto) ); + + return sizeof(QCQMUX_MSG_HDR) + sizeof(*pWdsAdminQosTlv) + sizeof(*linkProto) + sizeof(*dlTlp); + } + else { + //Indicates whether the Quality of Service(QOS) data format is used by the client. + pMUXMsg->SetDataFormatReq.QosDataFormatTlv.TLVType = 0x10; + pMUXMsg->SetDataFormatReq.QosDataFormatTlv.TLVLength = cpu_to_le16(0x0001); + pMUXMsg->SetDataFormatReq.QosDataFormatTlv.QOSSetting = 0; /* no-QOS header */ + //Underlying Link Layer Protocol + pMUXMsg->SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.TLVType = 0x11; + pMUXMsg->SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.TLVLength = cpu_to_le16(4); + pMUXMsg->SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.Value = cpu_to_le32(0x02); /* Set IP mode */ + //Uplink (UL) data aggregation protocol to be used for uplink data transfer. + pMUXMsg->SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVType = 0x12; + pMUXMsg->SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVLength = cpu_to_le16(4); + pMUXMsg->SetDataFormatReq.UplinkDataAggregationProtocolTlv.Value = cpu_to_le32(0x05); //UL QMAP is enabled + //Downlink (DL) data aggregation protocol to be used for downlink data transfer + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationProtocolTlv.TLVType = 0x13; + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationProtocolTlv.TLVLength = cpu_to_le16(4); + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationProtocolTlv.Value = cpu_to_le32(0x05); //UL QMAP is enabled + //Maximum number of datagrams in a single aggregated packet on downlink + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.TLVType = 0x15; + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.TLVLength = cpu_to_le16(4); + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.Value = cpu_to_le32(qmap_settings->rx_urb_size/512); + //Maximum size in bytes of a single aggregated packet allowed on downlink + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.TLVType = 0x16; + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.TLVLength = cpu_to_le16(4); + pMUXMsg->SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.Value = cpu_to_le32(qmap_settings->rx_urb_size); + //Peripheral End Point ID + pMUXMsg->SetDataFormatReq.epTlv.TLVType = 0x17; + pMUXMsg->SetDataFormatReq.epTlv.TLVLength = cpu_to_le16(8); + pMUXMsg->SetDataFormatReq.epTlv.ep_type = cpu_to_le32(qmap_settings->ep_type); + pMUXMsg->SetDataFormatReq.epTlv.iface_id = cpu_to_le32(qmap_settings->iface_id); + + return sizeof(QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG); + } +} + +#ifdef CONFIG_SIM +static USHORT DmsUIMVerifyPinReqSend(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->UIMVerifyPinReq.TLVType = 0x01; + pMUXMsg->UIMVerifyPinReq.PINID = 0x01; //Pin1, not Puk + pMUXMsg->UIMVerifyPinReq.PINLen = strlen((const char *)arg); + qstrcpy((PCHAR)&pMUXMsg->UIMVerifyPinReq.PINValue, ((const char *)arg)); + pMUXMsg->UIMVerifyPinReq.TLVLength = cpu_to_le16(2 + strlen((const char *)arg)); + return sizeof(QMIDMS_UIM_VERIFY_PIN_REQ_MSG) + (strlen((const char *)arg) - 1); +} + +static USHORT UimVerifyPinReqSend(PQMUX_MSG pMUXMsg, void *arg) +{ + pMUXMsg->UIMUIMVerifyPinReq.TLVType = 0x01; + pMUXMsg->UIMUIMVerifyPinReq.TLVLength = cpu_to_le16(0x02); + pMUXMsg->UIMUIMVerifyPinReq.Session_Type = 0x00; + pMUXMsg->UIMUIMVerifyPinReq.Aid_Len = 0x00; + pMUXMsg->UIMUIMVerifyPinReq.TLV2Type = 0x02; + pMUXMsg->UIMUIMVerifyPinReq.TLV2Length = cpu_to_le16(2 + strlen((const char *)arg)); + pMUXMsg->UIMUIMVerifyPinReq.PINID = 0x01; //Pin1, not Puk + pMUXMsg->UIMUIMVerifyPinReq.PINLen= strlen((const char *)arg); + qstrcpy((PCHAR)&pMUXMsg->UIMUIMVerifyPinReq.PINValue, ((const char *)arg)); + return sizeof(QMIUIM_VERIFY_PIN_REQ_MSG) + (strlen((const char *)arg) - 1); +} + +#ifdef CONFIG_IMSI_ICCID +static USHORT UimReadTransparentIMSIReqSend(PQMUX_MSG pMUXMsg, void *arg) { + PREAD_TRANSPARENT_TLV pReadTransparent; + + pMUXMsg->UIMUIMReadTransparentReq.TLVType = 0x01; + pMUXMsg->UIMUIMReadTransparentReq.TLVLength = cpu_to_le16(0x02); + if (!strcmp((char *)arg, "EF_ICCID")) { + pMUXMsg->UIMUIMReadTransparentReq.Session_Type = 0x06; + pMUXMsg->UIMUIMReadTransparentReq.Aid_Len = 0x00; + + pMUXMsg->UIMUIMReadTransparentReq.TLV2Type = 0x02; + pMUXMsg->UIMUIMReadTransparentReq.file_id = cpu_to_le16(0x2FE2); + pMUXMsg->UIMUIMReadTransparentReq.path_len = 0x02; + pMUXMsg->UIMUIMReadTransparentReq.path[0] = 0x00; + pMUXMsg->UIMUIMReadTransparentReq.path[1] = 0x3F; + } + else if(!strcmp((char *)arg, "EF_IMSI")) { + pMUXMsg->UIMUIMReadTransparentReq.Session_Type = 0x00; + pMUXMsg->UIMUIMReadTransparentReq.Aid_Len = 0x00; + + pMUXMsg->UIMUIMReadTransparentReq.TLV2Type = 0x02; + pMUXMsg->UIMUIMReadTransparentReq.file_id = cpu_to_le16(0x6F07); + pMUXMsg->UIMUIMReadTransparentReq.path_len = 0x04; + pMUXMsg->UIMUIMReadTransparentReq.path[0] = 0x00; + pMUXMsg->UIMUIMReadTransparentReq.path[1] = 0x3F; + pMUXMsg->UIMUIMReadTransparentReq.path[2] = 0xFF; + pMUXMsg->UIMUIMReadTransparentReq.path[3] = 0x7F; + } + + pMUXMsg->UIMUIMReadTransparentReq.TLV2Length = cpu_to_le16(3 + pMUXMsg->UIMUIMReadTransparentReq.path_len); + + pReadTransparent = (PREAD_TRANSPARENT_TLV)(&pMUXMsg->UIMUIMReadTransparentReq.path[pMUXMsg->UIMUIMReadTransparentReq.path_len]); + pReadTransparent->TLVType = 0x03; + pReadTransparent->TLVLength = cpu_to_le16(0x04); + pReadTransparent->Offset = cpu_to_le16(0x00); + pReadTransparent->Length = cpu_to_le16(0x00); + + return (sizeof(QMIUIM_READ_TRANSPARENT_REQ_MSG) + pMUXMsg->UIMUIMReadTransparentReq.path_len + sizeof(READ_TRANSPARENT_TLV)); +} +#endif +#endif + +#ifdef CONFIG_APN +static USHORT WdsGetProfileSettingsReqSend(PQMUX_MSG pMUXMsg, void *arg) { + PROFILE_T *profile = (PROFILE_T *)arg; + pMUXMsg->GetProfileSettingsReq.Length = cpu_to_le16(sizeof(QMIWDS_GET_PROFILE_SETTINGS_REQ_MSG) - 4); + pMUXMsg->GetProfileSettingsReq.TLVType = 0x01; + pMUXMsg->GetProfileSettingsReq.TLVLength = cpu_to_le16(0x02); + pMUXMsg->GetProfileSettingsReq.ProfileType = 0x00; // 0 ~ 3GPP, 1 ~ 3GPP2 + pMUXMsg->GetProfileSettingsReq.ProfileIndex = profile->pdp; + return sizeof(QMIWDS_GET_PROFILE_SETTINGS_REQ_MSG); +} + +static USHORT WdsModifyProfileSettingsReq(PQMUX_MSG pMUXMsg, void *arg) { + USHORT TLVLength = 0; + UCHAR *pTLV; + PROFILE_T *profile = (PROFILE_T *)arg; + PQMIWDS_PDPTYPE pPdpType; + + pMUXMsg->ModifyProfileSettingsReq.Length = cpu_to_le16(sizeof(QMIWDS_MODIFY_PROFILE_SETTINGS_REQ_MSG) - 4); + pMUXMsg->ModifyProfileSettingsReq.TLVType = 0x01; + pMUXMsg->ModifyProfileSettingsReq.TLVLength = cpu_to_le16(0x02); + pMUXMsg->ModifyProfileSettingsReq.ProfileType = 0x00; // 0 ~ 3GPP, 1 ~ 3GPP2 + pMUXMsg->ModifyProfileSettingsReq.ProfileIndex = profile->pdp; + + pTLV = (UCHAR *)(&pMUXMsg->ModifyProfileSettingsReq + 1); + + pPdpType = (PQMIWDS_PDPTYPE)(pTLV + TLVLength); + pPdpType->TLVType = 0x11; + pPdpType->TLVLength = cpu_to_le16(0x01); +// 0 ?C PDP-IP (IPv4) +// 1 ?C PDP-PPP +// 2 ?C PDP-IPv6 +// 3 ?C PDP-IPv4v6 + if (profile->IsDualIPSupported) + pPdpType->PdpType = 3; + else + pPdpType->PdpType = 0; + TLVLength +=(le16_to_cpu(pPdpType->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + + // Set APN Name + if (profile->apn) { + PQMIWDS_APNNAME pApnName = (PQMIWDS_APNNAME)(pTLV + TLVLength); + pApnName->TLVType = 0x14; + pApnName->TLVLength = cpu_to_le16(strlen(profile->apn)); + qstrcpy((char *)&pApnName->ApnName, profile->apn); + TLVLength +=(le16_to_cpu(pApnName->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Set User Name + if (profile->user) { + PQMIWDS_USERNAME pUserName = (PQMIWDS_USERNAME)(pTLV + TLVLength); + pUserName->TLVType = 0x1B; + pUserName->TLVLength = cpu_to_le16(strlen(profile->user)); + qstrcpy((char *)&pUserName->UserName, profile->user); + TLVLength += (le16_to_cpu(pUserName->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Set Password + if (profile->password) { + PQMIWDS_PASSWD pPasswd = (PQMIWDS_PASSWD)(pTLV + TLVLength); + pPasswd->TLVType = 0x1C; + pPasswd->TLVLength = cpu_to_le16(strlen(profile->password)); + qstrcpy((char *)&pPasswd->Passwd, profile->password); + TLVLength +=(le16_to_cpu(pPasswd->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + // Set Auth Protocol + if (profile->user && profile->password) { + PQMIWDS_AUTH_PREFERENCE pAuthPref = (PQMIWDS_AUTH_PREFERENCE)(pTLV + TLVLength); + pAuthPref->TLVType = 0x1D; + pAuthPref->TLVLength = cpu_to_le16(0x01); + pAuthPref->AuthPreference = profile->auth; // 0 ~ None, 1 ~ Pap, 2 ~ Chap, 3 ~ MsChapV2 + TLVLength += (le16_to_cpu(pAuthPref->TLVLength) + sizeof(QCQMICTL_TLV_HDR)); + } + + return sizeof(QMIWDS_MODIFY_PROFILE_SETTINGS_REQ_MSG) + TLVLength; +} +#endif + +static USHORT WdsGetRuntimeSettingReq(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->GetRuntimeSettingsReq.TLVType = 0x10; + pMUXMsg->GetRuntimeSettingsReq.TLVLength = cpu_to_le16(0x04); + // the following mask also applies to IPV6 + pMUXMsg->GetRuntimeSettingsReq.Mask = cpu_to_le32(QMIWDS_GET_RUNTIME_SETTINGS_MASK_IPV4DNS_ADDR | + QMIWDS_GET_RUNTIME_SETTINGS_MASK_IPV4_ADDR | + QMIWDS_GET_RUNTIME_SETTINGS_MASK_MTU | + QMIWDS_GET_RUNTIME_SETTINGS_MASK_IPV4GATEWAY_ADDR); // | + // QMIWDS_GET_RUNTIME_SETTINGS_MASK_PCSCF_SV_ADDR | + // QMIWDS_GET_RUNTIME_SETTINGS_MASK_PCSCF_DOM_NAME; + + return sizeof(QMIWDS_GET_RUNTIME_SETTINGS_REQ_MSG); +} + +static PQCQMIMSG s_pRequest; +static PQCQMIMSG s_pResponse; +static pthread_mutex_t s_commandmutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t s_commandcond = PTHREAD_COND_INITIALIZER; + +static int is_response(const PQCQMIMSG pRequest, const PQCQMIMSG pResponse) { + if ((pRequest->QMIHdr.QMIType == pResponse->QMIHdr.QMIType) + && (pRequest->QMIHdr.ClientId == pResponse->QMIHdr.ClientId)) { + USHORT requestTID, responseTID; + if (pRequest->QMIHdr.QMIType == QMUX_TYPE_CTL) { + requestTID = pRequest->CTLMsg.QMICTLMsgHdr.TransactionId; + responseTID = pResponse->CTLMsg.QMICTLMsgHdr.TransactionId; + } else { + requestTID = le16_to_cpu(pRequest->MUXMsg.QMUXHdr.TransactionId); + responseTID = le16_to_cpu(pResponse->MUXMsg.QMUXHdr.TransactionId); + } + return (requestTID == responseTID); + } + return 0; +} + +int QmiThreadSendQMITimeout(PQCQMIMSG pRequest, PQCQMIMSG *ppResponse, unsigned msecs) { + int ret; + + if (!pRequest) + { + return -EINVAL; + } + + pthread_mutex_lock(&s_commandmutex); + + if (ppResponse) + *ppResponse = NULL; + + dump_qmi(pRequest, le16_to_cpu(pRequest->QMIHdr.Length) + 1); + + s_pRequest = pRequest; + s_pResponse = NULL; + + if (!strncmp(qmichannel, "/dev/qcqmi", strlen("/dev/qcqmi"))) + ret = GobiNetSendQMI(pRequest); + else + ret = QmiWwanSendQMI(pRequest); + + if (ret == 0) { + ret = pthread_cond_timeout_np(&s_commandcond, &s_commandmutex, msecs); + if (!ret) { + if (s_pResponse && ppResponse) { + *ppResponse = s_pResponse; + } else { + if (s_pResponse) { + free(s_pResponse); + s_pResponse = NULL; + } + } + } else { + dbg_time("%s pthread_cond_timeout_np=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + } + } + + pthread_mutex_unlock(&s_commandmutex); + + return ret; +} + +int QmiThreadSendQMI(PQCQMIMSG pRequest, PQCQMIMSG *ppResponse) { + return QmiThreadSendQMITimeout(pRequest, ppResponse, 30 * 1000); +} + +void QmiThreadRecvQMI(PQCQMIMSG pResponse) { + pthread_mutex_lock(&s_commandmutex); + if (pResponse == NULL) { + if (s_pRequest) { + free(s_pRequest); + s_pRequest = NULL; + s_pResponse = NULL; + pthread_cond_signal(&s_commandcond); + } + pthread_mutex_unlock(&s_commandmutex); + return; + } + dump_qmi(pResponse, le16_to_cpu(pResponse->QMIHdr.Length) + 1); + if (s_pRequest && is_response(s_pRequest, pResponse)) { + free(s_pRequest); + s_pRequest = NULL; + s_pResponse = malloc(le16_to_cpu(pResponse->QMIHdr.Length) + 1); + if (s_pResponse != NULL) { + memcpy(s_pResponse, pResponse, le16_to_cpu(pResponse->QMIHdr.Length) + 1); + } + pthread_cond_signal(&s_commandcond); + } else if ((pResponse->QMIHdr.QMIType == QMUX_TYPE_NAS) + && (le16_to_cpu(pResponse->MUXMsg.QMUXMsgHdrResp.Type) == QMINAS_SERVING_SYSTEM_IND)) { + qmidevice_send_event_to_main(RIL_UNSOL_RESPONSE_VOICE_NETWORK_STATE_CHANGED); + } else if ((pResponse->QMIHdr.QMIType == QMUX_TYPE_WDS) + && (le16_to_cpu(pResponse->MUXMsg.QMUXMsgHdrResp.Type) == QMIWDS_GET_PKT_SRVC_STATUS_IND)) { + qmidevice_send_event_to_main(RIL_UNSOL_DATA_CALL_LIST_CHANGED); + } else if ((pResponse->QMIHdr.QMIType == QMUX_TYPE_NAS) + && (le16_to_cpu(pResponse->MUXMsg.QMUXMsgHdrResp.Type) == QMINAS_SYS_INFO_IND)) { + qmidevice_send_event_to_main(RIL_UNSOL_RESPONSE_VOICE_NETWORK_STATE_CHANGED); + } else { + if (debug_qmi) + dbg_time("nobody care this qmi msg!!"); + } + pthread_mutex_unlock(&s_commandmutex); +} + +int requestSetEthMode(PROFILE_T *profile) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse = NULL; + PQMUX_MSG pMUXMsg; + int err; + PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV linkProto; + UCHAR IpPreference; + UCHAR autoconnect_setting = 0; + QMAP_SETTING qmap_settings = {0, 0, 0, 0}; + + if (profile->qmapnet_adapter) { + profile->rawIP = 1; + s_9x07 = profile->rawIP; + qmap_settings.MuxId = (profile->qmapnet_adapter != profile->usbnet_adapter) ? (0x80 + profile->pdp) : 0x81; + + if (!strncmp(profile->qmichannel, "/dev/mhi_QMI", strlen("/dev/mhi_QMI"))) { //SDX20_PCIE + qmap_settings.rx_urb_size = 16*1024; //must same as rx_urb_size defined in GobiNet&qmi_wwan driver + qmap_settings.ep_type = DATA_EP_TYPE_PCIE; + qmap_settings.iface_id = 0x04; + } + else { // for MDM9x07&MDM9x40&SDX20 USB + qmap_settings.rx_urb_size = 16*1024; //must same as rx_urb_size defined in GobiNet&qmi_wwan driver + qmap_settings.ep_type = DATA_EP_TYPE_HSUSB; + qmap_settings.iface_id = 0x04; + } + + if (!strncmp(qmichannel, "/dev/qcqmi", strlen("/dev/qcqmi"))) //GobiNet set data format in GobiNet driver + goto skip_WdaSetDataFormat; + + if (profile->qmapnet_adapter != profile->usbnet_adapter) //QMAP MUX enabled, set data format in quectel-qmi-proxy + goto skip_WdaSetDataFormat; + } + + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS_ADMIN, QMIWDS_ADMIN_SET_DATA_FORMAT_REQ, WdaSetDataFormat, (void *)&qmap_settings); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + linkProto = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x11); + if (linkProto != NULL) { + profile->rawIP = (le32_to_cpu(linkProto->Value) == 2); + s_9x07 = profile->rawIP; + } + free(pResponse); + +skip_WdaSetDataFormat: + if (profile->qmapnet_adapter) { + // bind wds mux data port + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS, QMIWDS_BIND_MUX_DATA_PORT_REQ , WdsSetQMUXBindMuxDataPort, (void *)&qmap_settings); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + if (pResponse) free(pResponse); + } + + // set ipv4 + IpPreference = IpFamilyV4; + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS, QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ, WdsSetClientIPFamilyPref, (void *)&IpPreference); + err = QmiThreadSendQMI(pRequest, &pResponse); + if (pResponse) free(pResponse); + + if (profile->IsDualIPSupported) { + if (profile->qmapnet_adapter) { + // bind wds ipv6 mux data port + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS_IPV6, QMIWDS_BIND_MUX_DATA_PORT_REQ , WdsSetQMUXBindMuxDataPort, (void *)&qmap_settings); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + if (pResponse) free(pResponse); + } + + // set ipv6 + IpPreference = IpFamilyV6; + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS_IPV6, QMIWDS_SET_CLIENT_IP_FAMILY_PREF_REQ, WdsSetClientIPFamilyPref, (void *)&IpPreference); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + if (pResponse) free(pResponse); + } + + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS, QMIWDS_SET_AUTO_CONNECT_REQ , WdsSetAutoConnect, (void *)&autoconnect_setting); + QmiThreadSendQMI(pRequest, &pResponse); + if (pResponse) free(pResponse); + + return 0; +} + +#ifdef CONFIG_SIM +int requestGetPINStatus(SIM_Status *pSIMStatus) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + PQMIDMS_UIM_PIN_STATUS pPin1Status = NULL; + //PQMIDMS_UIM_PIN_STATUS pPin2Status = NULL; + + if (s_9x07 && qmiclientId[QMUX_TYPE_UIM]) + pRequest = ComposeQMUXMsg(QMUX_TYPE_UIM, QMIUIM_GET_CARD_STATUS_REQ, NULL, NULL); + else + pRequest = ComposeQMUXMsg(QMUX_TYPE_DMS, QMIDMS_UIM_GET_PIN_STATUS_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + pPin1Status = (PQMIDMS_UIM_PIN_STATUS)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x11); + //pPin2Status = (PQMIDMS_UIM_PIN_STATUS)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x12); + + if (pPin1Status != NULL) { + if (pPin1Status->PINStatus == QMI_PIN_STATUS_NOT_VERIF) { + *pSIMStatus = SIM_PIN; + } else if (pPin1Status->PINStatus == QMI_PIN_STATUS_BLOCKED) { + *pSIMStatus = SIM_PUK; + } else if (pPin1Status->PINStatus == QMI_PIN_STATUS_PERM_BLOCKED) { + *pSIMStatus = SIM_BAD; + } + } + + free(pResponse); + return 0; +} + +int requestGetSIMStatus(SIM_Status *pSIMStatus) { //RIL_REQUEST_GET_SIM_STATUS + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + const char * SIM_Status_String[] = { + "SIM_ABSENT", + "SIM_NOT_READY", + "SIM_READY", /* SIM_READY means the radio state is RADIO_STATE_SIM_READY */ + "SIM_PIN", + "SIM_PUK", + "SIM_NETWORK_PERSONALIZATION" + }; + + if (s_9x07 && qmiclientId[QMUX_TYPE_UIM]) + pRequest = ComposeQMUXMsg(QMUX_TYPE_UIM, QMIUIM_GET_CARD_STATUS_REQ, NULL, NULL); + else + pRequest = ComposeQMUXMsg(QMUX_TYPE_DMS, QMIDMS_UIM_GET_STATE_REQ, NULL, NULL); + + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + *pSIMStatus = SIM_ABSENT; + if (s_9x07 && qmiclientId[QMUX_TYPE_UIM]) + { + PQMIUIM_CARD_STATUS pCardStatus = NULL; + PQMIUIM_PIN_STATE pPINState = NULL; + UCHAR CardState = 0x01; + UCHAR PIN1State = QMI_PIN_STATUS_NOT_VERIF; + //UCHAR PIN1Retries; + //UCHAR PUK1Retries; + //UCHAR PIN2State; + //UCHAR PIN2Retries; + //UCHAR PUK2Retries; + + pCardStatus = (PQMIUIM_CARD_STATUS)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x10); + if (pCardStatus != NULL) + { + pPINState = (PQMIUIM_PIN_STATE)((PUCHAR)pCardStatus + sizeof(QMIUIM_CARD_STATUS) + pCardStatus->AIDLength); + CardState = pCardStatus->CardState; + if (pPINState->UnivPIN == 1) + { + PIN1State = pCardStatus->UPINState; + //PIN1Retries = pCardStatus->UPINRetries; + //PUK1Retries = pCardStatus->UPUKRetries; + } + else + { + PIN1State = pPINState->PIN1State; + //PIN1Retries = pPINState->PIN1Retries; + //PUK1Retries = pPINState->PUK1Retries; + } + //PIN2State = pPINState->PIN2State; + //PIN2Retries = pPINState->PIN2Retries; + //PUK2Retries = pPINState->PUK2Retries; + } + + *pSIMStatus = SIM_ABSENT; + if ((CardState == 0x01) && ((PIN1State == QMI_PIN_STATUS_VERIFIED)|| (PIN1State == QMI_PIN_STATUS_DISABLED))) + { + *pSIMStatus = SIM_READY; + } + else if (CardState == 0x01) + { + if (PIN1State == QMI_PIN_STATUS_NOT_VERIF) + { + *pSIMStatus = SIM_PIN; + } + if ( PIN1State == QMI_PIN_STATUS_BLOCKED) + { + *pSIMStatus = SIM_PUK; + } + else if (PIN1State == QMI_PIN_STATUS_PERM_BLOCKED) + { + *pSIMStatus = SIM_BAD; + } + else if (PIN1State == QMI_PIN_STATUS_NOT_INIT || PIN1State == QMI_PIN_STATUS_VERIFIED || PIN1State == QMI_PIN_STATUS_DISABLED) + { + *pSIMStatus = SIM_READY; + } + } + else if (CardState == 0x00 || CardState == 0x02) + { + } + else + { + } + } + else + { + //UIM state. Values: + // 0x00 UIM initialization completed + // 0x01 UIM is locked or the UIM failed + // 0x02 UIM is not present + // 0x03 Reserved + // 0xFF UIM state is currently + //unavailable + if (pResponse->MUXMsg.UIMGetStateResp.UIMState == 0x00) { + *pSIMStatus = SIM_READY; + } else if (pResponse->MUXMsg.UIMGetStateResp.UIMState == 0x01) { + *pSIMStatus = SIM_ABSENT; + err = requestGetPINStatus(pSIMStatus); + } else if ((pResponse->MUXMsg.UIMGetStateResp.UIMState == 0x02) || (pResponse->MUXMsg.UIMGetStateResp.UIMState == 0xFF)) { + *pSIMStatus = SIM_ABSENT; + } else { + *pSIMStatus = SIM_ABSENT; + } + } + dbg_time("%s SIMStatus: %s", __func__, SIM_Status_String[*pSIMStatus]); + + free(pResponse); + + return 0; +} + +int requestEnterSimPin(const CHAR *pPinCode) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + + if (s_9x07 && qmiclientId[QMUX_TYPE_UIM]) + pRequest = ComposeQMUXMsg(QMUX_TYPE_UIM, QMIUIM_VERIFY_PIN_REQ, UimVerifyPinReqSend, (void *)pPinCode); + else + pRequest = ComposeQMUXMsg(QMUX_TYPE_DMS, QMIDMS_UIM_VERIFY_PIN_REQ, DmsUIMVerifyPinReqSend, (void *)pPinCode); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + free(pResponse); + return 0; +} + +#ifdef CONFIG_IMSI_ICCID +int requestGetICCID(void) { //RIL_REQUEST_GET_IMSI + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + PQMIUIM_CONTENT pUimContent; + int err; + + if (s_9x07 && qmiclientId[QMUX_TYPE_UIM]) { + pRequest = ComposeQMUXMsg(QMUX_TYPE_UIM, QMIUIM_READ_TRANSPARENT_REQ, UimReadTransparentIMSIReqSend, (void *)"EF_ICCID"); + err = QmiThreadSendQMI(pRequest, &pResponse); + } else { + return 0; + } + qmi_rsp_check_and_return(); + + pUimContent = (PQMIUIM_CONTENT)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x11); + if (pUimContent != NULL) { + static char DeviceICCID[32] = {'\0'}; + int i = 0, j = 0; + + for (i = 0, j = 0; i < le16_to_cpu(pUimContent->content_len); ++i) { + char charmaps[] = "0123456789ABCDEF"; + + DeviceICCID[j++] = charmaps[(pUimContent->content[i] & 0x0F)]; + DeviceICCID[j++] = charmaps[((pUimContent->content[i] & 0xF0) >> 0x04)]; + } + DeviceICCID[j] = '\0'; + + dbg_time("%s DeviceICCID: %s", __func__, DeviceICCID); + } + + free(pResponse); + return 0; +} + +int requestGetIMSI(void) { //RIL_REQUEST_GET_IMSI + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + PQMIUIM_CONTENT pUimContent; + int err; + + if (s_9x07 && qmiclientId[QMUX_TYPE_UIM]) { + pRequest = ComposeQMUXMsg(QMUX_TYPE_UIM, QMIUIM_READ_TRANSPARENT_REQ, UimReadTransparentIMSIReqSend, (void *)"EF_IMSI"); + err = QmiThreadSendQMI(pRequest, &pResponse); + } else { + return 0; + } + qmi_rsp_check_and_return(); + + pUimContent = (PQMIUIM_CONTENT)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x11); + if (pUimContent != NULL) { + static char DeviceIMSI[32] = {'\0'}; + int i = 0, j = 0; + + for (i = 0, j = 0; i < le16_to_cpu(pUimContent->content[0]); ++i) { + if (i != 0) + DeviceIMSI[j++] = (pUimContent->content[i+1] & 0x0F) + '0'; + DeviceIMSI[j++] = ((pUimContent->content[i+1] & 0xF0) >> 0x04) + '0'; + } + DeviceIMSI[j] = '\0'; + + dbg_time("%s DeviceIMSI: %s", __func__, DeviceIMSI); + } + + free(pResponse); + return 0; +} +#endif +#endif + +#if 1 +static void quectel_convert_cdma_mcc_2_ascii_mcc( USHORT *p_mcc, USHORT mcc ) +{ + unsigned int d1, d2, d3, buf = mcc + 111; + + if ( mcc == 0x3FF ) // wildcard + { + *p_mcc = 3; + } + else + { + d3 = buf % 10; + buf = ( d3 == 0 ) ? (buf-10)/10 : buf/10; + + d2 = buf % 10; + buf = ( d2 == 0 ) ? (buf-10)/10 : buf/10; + + d1 = ( buf == 10 ) ? 0 : buf; + +//dbg_time("d1:%d, d2:%d,d3:%d",d1,d2,d3); + if ( d1<10 && d2<10 && d3<10 ) + { + *p_mcc = d1*100+d2*10+d3; +#if 0 + *(p_mcc+0) = '0' + d1; + *(p_mcc+1) = '0' + d2; + *(p_mcc+2) = '0' + d3; +#endif + } + else + { + //dbg_time( "invalid digits %d %d %d", d1, d2, d3 ); + *p_mcc = 0; + } + } +} + +static void quectel_convert_cdma_mnc_2_ascii_mnc( USHORT *p_mnc, USHORT imsi_11_12) +{ + unsigned int d1, d2, buf = imsi_11_12 + 11; + + if ( imsi_11_12 == 0x7F ) // wildcard + { + *p_mnc = 7; + } + else + { + d2 = buf % 10; + buf = ( d2 == 0 ) ? (buf-10)/10 : buf/10; + + d1 = ( buf == 10 ) ? 0 : buf; + + if ( d1<10 && d2<10 ) + { + *p_mnc = d1*10 + d2; + } + else + { + //dbg_time( "invalid digits %d %d", d1, d2, 0 ); + *p_mnc = 0; + } + } +} + +int requestGetHomeNetwork(USHORT *p_mcc, USHORT *p_mnc, USHORT *p_sid, USHORT *p_nid) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + PHOME_NETWORK pHomeNetwork; + PHOME_NETWORK_SYSTEMID pHomeNetworkSystemID; + + pRequest = ComposeQMUXMsg(QMUX_TYPE_NAS, QMINAS_GET_HOME_NETWORK_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + pHomeNetwork = (PHOME_NETWORK)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x01); + if (pHomeNetwork && p_mcc && p_mnc ) { + *p_mcc = le16_to_cpu(pHomeNetwork->MobileCountryCode); + *p_mnc = le16_to_cpu(pHomeNetwork->MobileNetworkCode); + //dbg_time("%s MobileCountryCode: %d, MobileNetworkCode: %d", __func__, *pMobileCountryCode, *pMobileNetworkCode); + } + + pHomeNetworkSystemID = (PHOME_NETWORK_SYSTEMID)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x10); + if (pHomeNetworkSystemID && p_sid && p_nid) { + *p_sid = le16_to_cpu(pHomeNetworkSystemID->SystemID); //china-hefei: sid 14451 + *p_nid = le16_to_cpu(pHomeNetworkSystemID->NetworkID); + //dbg_time("%s SystemID: %d, NetworkID: %d", __func__, *pSystemID, *pNetworkID); + } + + free(pResponse); + + return 0; +} +#endif + +#if 0 +// Lookup table for carriers known to produce SIMs which incorrectly indicate MNC length. +static const char * MCCMNC_CODES_HAVING_3DIGITS_MNC[] = { + "302370", "302720", "310260", + "405025", "405026", "405027", "405028", "405029", "405030", "405031", "405032", + "405033", "405034", "405035", "405036", "405037", "405038", "405039", "405040", + "405041", "405042", "405043", "405044", "405045", "405046", "405047", "405750", + "405751", "405752", "405753", "405754", "405755", "405756", "405799", "405800", + "405801", "405802", "405803", "405804", "405805", "405806", "405807", "405808", + "405809", "405810", "405811", "405812", "405813", "405814", "405815", "405816", + "405817", "405818", "405819", "405820", "405821", "405822", "405823", "405824", + "405825", "405826", "405827", "405828", "405829", "405830", "405831", "405832", + "405833", "405834", "405835", "405836", "405837", "405838", "405839", "405840", + "405841", "405842", "405843", "405844", "405845", "405846", "405847", "405848", + "405849", "405850", "405851", "405852", "405853", "405875", "405876", "405877", + "405878", "405879", "405880", "405881", "405882", "405883", "405884", "405885", + "405886", "405908", "405909", "405910", "405911", "405912", "405913", "405914", + "405915", "405916", "405917", "405918", "405919", "405920", "405921", "405922", + "405923", "405924", "405925", "405926", "405927", "405928", "405929", "405930", + "405931", "405932", "502142", "502143", "502145", "502146", "502147", "502148" +}; + +static const char * MCC_CODES_HAVING_3DIGITS_MNC[] = { + "302", //Canada + "310", //United States of America + "311", //United States of America + "312", //United States of America + "313", //United States of America + "314", //United States of America + "315", //United States of America + "316", //United States of America + "334", //Mexico + "338", //Jamaica + "342", //Barbados + "344", //Antigua and Barbuda + "346", //Cayman Islands + "348", //British Virgin Islands + "365", //Anguilla + "708", //Honduras (Republic of) + "722", //Argentine Republic + "732" //Colombia (Republic of) +}; + +int requestGetIMSI(const char **pp_imsi, USHORT *pMobileCountryCode, USHORT *pMobileNetworkCode) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + + if (pp_imsi) *pp_imsi = NULL; + if (pMobileCountryCode) *pMobileCountryCode = 0; + if (pMobileNetworkCode) *pMobileNetworkCode = 0; + + pRequest = ComposeQMUXMsg(QMUX_TYPE_DMS, QMIDMS_UIM_GET_IMSI_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + if (pMUXMsg->UIMGetIMSIResp.TLV2Type == 0x01 && le16_to_cpu(pMUXMsg->UIMGetIMSIResp.TLV2Length) >= 5) { + int mnc_len = 2; + unsigned i; + char tmp[4]; + + if (pp_imsi) *pp_imsi = strndup((const char *)(&pMUXMsg->UIMGetIMSIResp.IMSI), le16_to_cpu(pMUXMsg->UIMGetIMSIResp.TLV2Length)); + + for (i = 0; i < sizeof(MCCMNC_CODES_HAVING_3DIGITS_MNC)/sizeof(MCCMNC_CODES_HAVING_3DIGITS_MNC[0]); i++) { + if (!strncmp((const char *)(&pMUXMsg->UIMGetIMSIResp.IMSI), MCCMNC_CODES_HAVING_3DIGITS_MNC[i], 6)) { + mnc_len = 3; + break; + } + } + if (mnc_len == 2) { + for (i = 0; i < sizeof(MCC_CODES_HAVING_3DIGITS_MNC)/sizeof(MCC_CODES_HAVING_3DIGITS_MNC[0]); i++) { + if (!strncmp((const char *)(&pMUXMsg->UIMGetIMSIResp.IMSI), MCC_CODES_HAVING_3DIGITS_MNC[i], 3)) { + mnc_len = 3; + break; + } + } + } + + tmp[0] = (&pMUXMsg->UIMGetIMSIResp.IMSI)[0]; + tmp[1] = (&pMUXMsg->UIMGetIMSIResp.IMSI)[1]; + tmp[2] = (&pMUXMsg->UIMGetIMSIResp.IMSI)[2]; + tmp[3] = 0; + if (pMobileCountryCode) *pMobileCountryCode = atoi(tmp); + tmp[0] = (&pMUXMsg->UIMGetIMSIResp.IMSI)[3]; + tmp[1] = (&pMUXMsg->UIMGetIMSIResp.IMSI)[4]; + tmp[2] = 0; + if (mnc_len == 3) { + tmp[2] = (&pMUXMsg->UIMGetIMSIResp.IMSI)[6]; + } + if (pMobileNetworkCode) *pMobileNetworkCode = atoi(tmp); + } + + free(pResponse); + + return 0; +} +#endif + +struct wwan_data_class_str class2str[] = { + {WWAN_DATA_CLASS_NONE, "UNKNOWN"}, + {WWAN_DATA_CLASS_GPRS, "GPRS"}, + {WWAN_DATA_CLASS_EDGE, "EDGE"}, + {WWAN_DATA_CLASS_UMTS, "UMTS"}, + {WWAN_DATA_CLASS_HSDPA, "HSDPA"}, + {WWAN_DATA_CLASS_HSUPA, "HSUPA"}, + {WWAN_DATA_CLASS_LTE, "LTE"}, + {WWAN_DATA_CLASS_1XRTT, "1XRTT"}, + {WWAN_DATA_CLASS_1XEVDO, "1XEVDO"}, + {WWAN_DATA_CLASS_1XEVDO_REVA, "1XEVDO_REVA"}, + {WWAN_DATA_CLASS_1XEVDV, "1XEVDV"}, + {WWAN_DATA_CLASS_3XRTT, "3XRTT"}, + {WWAN_DATA_CLASS_1XEVDO_REVB, "1XEVDO_REVB"}, + {WWAN_DATA_CLASS_UMB, "UMB"}, + {WWAN_DATA_CLASS_CUSTOM, "CUSTOM"}, +}; + +CHAR *wwan_data_class2str(ULONG class) +{ + unsigned int i = 0; + for (i = 0; i < sizeof(class2str)/sizeof(class2str[0]); i++) { + if (class2str[i].class == class) { + return class2str[i].str; + } + } + return "UNKNOWN"; +} + +int requestRegistrationState2(UCHAR *pPSAttachedState) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + USHORT MobileCountryCode = 0; + USHORT MobileNetworkCode = 0; + const char *pDataCapStr = "UNKNOW"; + LONG remainingLen; + PSERVICE_STATUS_INFO pServiceStatusInfo; + int is_lte = 0; + PCDMA_SYSTEM_INFO pCdmaSystemInfo; + PHDR_SYSTEM_INFO pHdrSystemInfo; + PGSM_SYSTEM_INFO pGsmSystemInfo; + PWCDMA_SYSTEM_INFO pWcdmaSystemInfo; + PLTE_SYSTEM_INFO pLteSystemInfo; + PTDSCDMA_SYSTEM_INFO pTdscdmaSystemInfo; + UCHAR DeviceClass = 0; + ULONG DataCapList = 0; + + *pPSAttachedState = 0; + pRequest = ComposeQMUXMsg(QMUX_TYPE_NAS, QMINAS_GET_SYS_INFO_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + pServiceStatusInfo = (PSERVICE_STATUS_INFO)(((PCHAR)&pMUXMsg->GetSysInfoResp) + QCQMUX_MSG_HDR_SIZE); + remainingLen = le16_to_cpu(pMUXMsg->GetSysInfoResp.Length); + + s_is_cdma = 0; + s_hdr_personality = 0; + while (remainingLen > 0) { + switch (pServiceStatusInfo->TLVType) { + case 0x10: // CDMA + if (pServiceStatusInfo->SrvStatus == 0x02) { + DataCapList = WWAN_DATA_CLASS_1XRTT| + WWAN_DATA_CLASS_1XEVDO| + WWAN_DATA_CLASS_1XEVDO_REVA| + WWAN_DATA_CLASS_1XEVDV| + WWAN_DATA_CLASS_1XEVDO_REVB; + DeviceClass = DEVICE_CLASS_CDMA; + s_is_cdma = (0 == is_lte); + } + break; + case 0x11: // HDR + if (pServiceStatusInfo->SrvStatus == 0x02) { + DataCapList = WWAN_DATA_CLASS_3XRTT| + WWAN_DATA_CLASS_UMB; + DeviceClass = DEVICE_CLASS_CDMA; + s_is_cdma = (0 == is_lte); + } + break; + case 0x12: // GSM + if (pServiceStatusInfo->SrvStatus == 0x02) { + DataCapList = WWAN_DATA_CLASS_GPRS| + WWAN_DATA_CLASS_EDGE; + DeviceClass = DEVICE_CLASS_GSM; + } + break; + case 0x13: // WCDMA + if (pServiceStatusInfo->SrvStatus == 0x02) { + DataCapList = WWAN_DATA_CLASS_UMTS; + DeviceClass = DEVICE_CLASS_GSM; + } + break; + case 0x14: // LTE + if (pServiceStatusInfo->SrvStatus == 0x02) { + DataCapList = WWAN_DATA_CLASS_LTE; + DeviceClass = DEVICE_CLASS_GSM; + is_lte = 1; + s_is_cdma = 0; + } + break; + case 0x24: // TDSCDMA + if (pServiceStatusInfo->SrvStatus == 0x02) { + pDataCapStr = "TD-SCDMA"; + } + break; + case 0x15: // CDMA + // CDMA_SYSTEM_INFO + pCdmaSystemInfo = (PCDMA_SYSTEM_INFO)pServiceStatusInfo; + if (pCdmaSystemInfo->SrvDomainValid == 0x01) { + *pPSAttachedState = 0; + if (pCdmaSystemInfo->SrvDomain & 0x02) { + *pPSAttachedState = 1; + s_is_cdma = (0 == is_lte); + } + } +#if 0 + if (pCdmaSystemInfo->SrvCapabilityValid == 0x01) { + *pPSAttachedState = 0; + if (pCdmaSystemInfo->SrvCapability & 0x02) { + *pPSAttachedState = 1; + s_is_cdma = (0 == is_lte); + } + } +#endif + if (pCdmaSystemInfo->NetworkIdValid == 0x01) { + int i; + CHAR temp[10]; + strncpy(temp, (CHAR *)pCdmaSystemInfo->MCC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileCountryCode = (USHORT)atoi(temp); + + strncpy(temp, (CHAR *)pCdmaSystemInfo->MNC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileNetworkCode = (USHORT)atoi(temp); + } + break; + case 0x16: // HDR + // HDR_SYSTEM_INFO + pHdrSystemInfo = (PHDR_SYSTEM_INFO)pServiceStatusInfo; + if (pHdrSystemInfo->SrvDomainValid == 0x01) { + *pPSAttachedState = 0; + if (pHdrSystemInfo->SrvDomain & 0x02) { + *pPSAttachedState = 1; + s_is_cdma = (0 == is_lte); + } + } +#if 0 + if (pHdrSystemInfo->SrvCapabilityValid == 0x01) { + *pPSAttachedState = 0; + if (pHdrSystemInfo->SrvCapability & 0x02) { + *pPSAttachedState = 1; + s_is_cdma = (0 == is_lte); + } + } +#endif + if (*pPSAttachedState && pHdrSystemInfo->HdrPersonalityValid == 0x01) { + if (pHdrSystemInfo->HdrPersonality == 0x03) + s_hdr_personality = 0x02; + //else if (pHdrSystemInfo->HdrPersonality == 0x02) + // s_hdr_personality = 0x01; + } + USHORT cmda_mcc = 0, cdma_mnc = 0; + if(!requestGetHomeNetwork(&cmda_mcc, &cdma_mnc,NULL, NULL) && cmda_mcc) { + quectel_convert_cdma_mcc_2_ascii_mcc(&MobileCountryCode, cmda_mcc); + quectel_convert_cdma_mnc_2_ascii_mnc(&MobileNetworkCode, cdma_mnc); + } + break; + case 0x17: // GSM + // GSM_SYSTEM_INFO + pGsmSystemInfo = (PGSM_SYSTEM_INFO)pServiceStatusInfo; + if (pGsmSystemInfo->SrvDomainValid == 0x01) { + *pPSAttachedState = 0; + if (pGsmSystemInfo->SrvDomain & 0x02) { + *pPSAttachedState = 1; + } + } +#if 0 + if (pGsmSystemInfo->SrvCapabilityValid == 0x01) { + *pPSAttachedState = 0; + if (pGsmSystemInfo->SrvCapability & 0x02) { + *pPSAttachedState = 1; + } + } +#endif + if (pGsmSystemInfo->NetworkIdValid == 0x01) { + int i; + CHAR temp[10]; + strncpy(temp, (CHAR *)pGsmSystemInfo->MCC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileCountryCode = (USHORT)atoi(temp); + + strncpy(temp, (CHAR *)pGsmSystemInfo->MNC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileNetworkCode = (USHORT)atoi(temp); + } + break; + case 0x18: // WCDMA + // WCDMA_SYSTEM_INFO + pWcdmaSystemInfo = (PWCDMA_SYSTEM_INFO)pServiceStatusInfo; + if (pWcdmaSystemInfo->SrvDomainValid == 0x01) { + *pPSAttachedState = 0; + if (pWcdmaSystemInfo->SrvDomain & 0x02) { + *pPSAttachedState = 1; + } + } +#if 0 + if (pWcdmaSystemInfo->SrvCapabilityValid == 0x01) { + *pPSAttachedState = 0; + if (pWcdmaSystemInfo->SrvCapability & 0x02) { + *pPSAttachedState = 1; + } + } +#endif + if (pWcdmaSystemInfo->NetworkIdValid == 0x01) { + int i; + CHAR temp[10]; + strncpy(temp, (CHAR *)pWcdmaSystemInfo->MCC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileCountryCode = (USHORT)atoi(temp); + + strncpy(temp, (CHAR *)pWcdmaSystemInfo->MNC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileNetworkCode = (USHORT)atoi(temp); + } + break; + case 0x19: // LTE_SYSTEM_INFO + // LTE_SYSTEM_INFO + pLteSystemInfo = (PLTE_SYSTEM_INFO)pServiceStatusInfo; + if (pLteSystemInfo->SrvDomainValid == 0x01) { + *pPSAttachedState = 0; + if (pLteSystemInfo->SrvDomain & 0x02) { + *pPSAttachedState = 1; + is_lte = 1; + s_is_cdma = 0; + } + } +#if 0 + if (pLteSystemInfo->SrvCapabilityValid == 0x01) { + *pPSAttachedState = 0; + if (pLteSystemInfo->SrvCapability & 0x02) { + *pPSAttachedState = 1; + is_lte = 1; + s_is_cdma = 0; + } + } +#endif + if (pLteSystemInfo->NetworkIdValid == 0x01) { + int i; + CHAR temp[10]; + strncpy(temp, (CHAR *)pLteSystemInfo->MCC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileCountryCode = (USHORT)atoi(temp); + + strncpy(temp, (CHAR *)pLteSystemInfo->MNC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileNetworkCode = (USHORT)atoi(temp); + } + break; + case 0x25: // TDSCDMA + // TDSCDMA_SYSTEM_INFO + pTdscdmaSystemInfo = (PTDSCDMA_SYSTEM_INFO)pServiceStatusInfo; + if (pTdscdmaSystemInfo->SrvDomainValid == 0x01) { + *pPSAttachedState = 0; + if (pTdscdmaSystemInfo->SrvDomain & 0x02) { + *pPSAttachedState = 1; + } + } +#if 0 + if (pTdscdmaSystemInfo->SrvCapabilityValid == 0x01) { + *pPSAttachedState = 0; + if (pTdscdmaSystemInfo->SrvCapability & 0x02) { + *pPSAttachedState = 1; + } + } +#endif + if (pTdscdmaSystemInfo->NetworkIdValid == 0x01) { + int i; + CHAR temp[10]; + strncpy(temp, (CHAR *)pTdscdmaSystemInfo->MCC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileCountryCode = (USHORT)atoi(temp); + + strncpy(temp, (CHAR *)pTdscdmaSystemInfo->MNC, 3); + temp[3] = '\0'; + for (i = 0; i < 4; i++) { + if ((UCHAR)temp[i] == 0xFF) { + temp[i] = '\0'; + } + } + MobileNetworkCode = (USHORT)atoi(temp); + } + break; + default: + break; + } /* switch (pServiceStatusInfo->TLYType) */ + remainingLen -= (le16_to_cpu(pServiceStatusInfo->TLVLength) + 3); + pServiceStatusInfo = (PSERVICE_STATUS_INFO)((PCHAR)&pServiceStatusInfo->TLVLength + le16_to_cpu(pServiceStatusInfo->TLVLength) + sizeof(USHORT)); + } /* while (remainingLen > 0) */ + + if (DeviceClass == DEVICE_CLASS_CDMA) { + if (s_hdr_personality == 2) { + pDataCapStr = s_hdr_personality == 2 ? "eHRPD" : "HRPD"; + } else if (DataCapList & WWAN_DATA_CLASS_1XEVDO_REVB) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_1XEVDO_REVB); + } else if (DataCapList & WWAN_DATA_CLASS_1XEVDO_REVA) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_1XEVDO_REVA); + } else if (DataCapList & WWAN_DATA_CLASS_1XEVDO) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_1XEVDO); + } else if (DataCapList & WWAN_DATA_CLASS_1XRTT) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_1XRTT); + } else if (DataCapList & WWAN_DATA_CLASS_3XRTT) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_3XRTT); + } else if (DataCapList & WWAN_DATA_CLASS_UMB) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_UMB); + } + } else { + if (DataCapList & WWAN_DATA_CLASS_LTE) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_LTE); + } else if ((DataCapList & WWAN_DATA_CLASS_HSDPA) && (DataCapList & WWAN_DATA_CLASS_HSUPA)) { + pDataCapStr = "HSDPA_HSUPA"; + } else if (DataCapList & WWAN_DATA_CLASS_HSDPA) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_HSDPA); + } else if (DataCapList & WWAN_DATA_CLASS_HSUPA) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_HSUPA); + } else if (DataCapList & WWAN_DATA_CLASS_UMTS) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_UMTS); + } else if (DataCapList & WWAN_DATA_CLASS_EDGE) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_EDGE); + } else if (DataCapList & WWAN_DATA_CLASS_GPRS) { + pDataCapStr = wwan_data_class2str(WWAN_DATA_CLASS_GPRS); + } + } + + dbg_time("%s MCC: %d, MNC: %d, PS: %s, DataCap: %s", __func__, + MobileCountryCode, MobileNetworkCode, (*pPSAttachedState == 1) ? "Attached" : "Detached" , pDataCapStr); + + free(pResponse); + + return 0; +} + +int requestRegistrationState(UCHAR *pPSAttachedState) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + PQMINAS_CURRENT_PLMN_MSG pCurrentPlmn; + PSERVING_SYSTEM pServingSystem; + PQMINAS_DATA_CAP pDataCap; + USHORT MobileCountryCode = 0; + USHORT MobileNetworkCode = 0; + const char *pDataCapStr = "UNKNOW"; + + if (s_9x07) { + return requestRegistrationState2(pPSAttachedState); + } + + pRequest = ComposeQMUXMsg(QMUX_TYPE_NAS, QMINAS_GET_SERVING_SYSTEM_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + pCurrentPlmn = (PQMINAS_CURRENT_PLMN_MSG)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x12); + if (pCurrentPlmn) { + MobileCountryCode = le16_to_cpu(pCurrentPlmn->MobileCountryCode); + MobileNetworkCode = le16_to_cpu(pCurrentPlmn->MobileNetworkCode); + } + + *pPSAttachedState = 0; + pServingSystem = (PSERVING_SYSTEM)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x01); + if (pServingSystem) { + //Packet-switched domain attach state of the mobile. + //0x00 PS_UNKNOWN ?Unknown or not applicable + //0x01 PS_ATTACHED ?Attached + //0x02 PS_DETACHED ?Detached + *pPSAttachedState = pServingSystem->RegistrationState; + if (pServingSystem->RegistrationState == 0x01) //0x01 ?C REGISTERED ?C Registered with a network + *pPSAttachedState = pServingSystem->PSAttachedState; + else { + //MobileCountryCode = MobileNetworkCode = 0; + *pPSAttachedState = 0x02; + } + } + + pDataCap = (PQMINAS_DATA_CAP)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x11); + if (pDataCap && pDataCap->DataCapListLen) { + UCHAR *DataCap = &pDataCap->DataCap; + if (pDataCap->DataCapListLen == 2) { + if ((DataCap[0] == 0x06) && ((DataCap[1] == 0x08) || (DataCap[1] == 0x0A))) + DataCap[0] = DataCap[1]; + } + switch (DataCap[0]) { + case 0x01: pDataCapStr = "GPRS"; break; + case 0x02: pDataCapStr = "EDGE"; break; + case 0x03: pDataCapStr = "HSDPA"; break; + case 0x04: pDataCapStr = "HSUPA"; break; + case 0x05: pDataCapStr = "UMTS"; break; + case 0x06: pDataCapStr = "1XRTT"; break; + case 0x07: pDataCapStr = "1XEVDO"; break; + case 0x08: pDataCapStr = "1XEVDO_REVA"; break; + case 0x09: pDataCapStr = "GPRS"; break; + case 0x0A: pDataCapStr = "1XEVDO_REVB"; break; + case 0x0B: pDataCapStr = "LTE"; break; + case 0x0C: pDataCapStr = "HSDPA"; break; + case 0x0D: pDataCapStr = "HSDPA"; break; + default: pDataCapStr = "UNKNOW"; break; + } + } + + if (pServingSystem && pServingSystem->RegistrationState == 0x01 && pServingSystem->InUseRadioIF && pServingSystem->RadioIF == 0x09) { + pDataCapStr = "TD-SCDMA"; + } + + s_is_cdma = 0; + if (pServingSystem && pServingSystem->RegistrationState == 0x01 && pServingSystem->InUseRadioIF && (pServingSystem->RadioIF == 0x01 || pServingSystem->RadioIF == 0x02)) { + USHORT cmda_mcc = 0, cdma_mnc = 0; + s_is_cdma = 1; + if(!requestGetHomeNetwork(&cmda_mcc, &cdma_mnc,NULL, NULL) && cmda_mcc) { + quectel_convert_cdma_mcc_2_ascii_mcc(&MobileCountryCode, cmda_mcc); + quectel_convert_cdma_mnc_2_ascii_mnc(&MobileNetworkCode, cdma_mnc); + } + if (1) { + PQCQMUX_TLV pTLV = (PQCQMUX_TLV)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x23); + if (pTLV) + s_hdr_personality = pTLV->Value; + else + s_hdr_personality = 0; + if (s_hdr_personality == 2) + pDataCapStr = "eHRPD"; + } + } + + dbg_time("%s MCC: %d, MNC: %d, PS: %s, DataCap: %s", __func__, + MobileCountryCode, MobileNetworkCode, (*pPSAttachedState == 1) ? "Attached" : "Detached" , pDataCapStr); + + free(pResponse); + + return 0; +} + +int requestQueryDataCall(UCHAR *pConnectionStatus, int curIpFamily) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + PQMIWDS_PKT_SRVC_TLV pPktSrvc; + UCHAR oldConnectionStatus = *pConnectionStatus; + UCHAR QMIType = (curIpFamily == IpFamilyV4) ? QMUX_TYPE_WDS : QMUX_TYPE_WDS_IPV6; + + pRequest = ComposeQMUXMsg(QMIType, QMIWDS_GET_PKT_SRVC_STATUS_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + *pConnectionStatus = QWDS_PKT_DATA_DISCONNECTED; + pPktSrvc = (PQMIWDS_PKT_SRVC_TLV)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x01); + if (pPktSrvc) { + *pConnectionStatus = pPktSrvc->ConnectionStatus; + if ((le16_to_cpu(pPktSrvc->TLVLength) == 2) && (pPktSrvc->ReconfigReqd == 0x01)) + *pConnectionStatus = QWDS_PKT_DATA_DISCONNECTED; + } + + if (*pConnectionStatus == QWDS_PKT_DATA_DISCONNECTED) { + if (curIpFamily == IpFamilyV4) + WdsConnectionIPv4Handle = 0; + else + WdsConnectionIPv6Handle = 0; + } + + if (oldConnectionStatus != *pConnectionStatus || debug_qmi) { + dbg_time("%s %sConnectionStatus: %s", __func__, (curIpFamily == IpFamilyV4) ? "IPv4" : "IPv6", + (*pConnectionStatus == QWDS_PKT_DATA_CONNECTED) ? "CONNECTED" : "DISCONNECTED"); + } + + free(pResponse); + return 0; +} + +#if 0 +BOOLEAN QCMAIN_IsDualIPSupported(PMP_ADAPTER pAdapter) +{ + return (pAdapter->QMUXVersion[QMUX_TYPE_WDS].Major >= 1 && pAdapter->QMUXVersion[QMUX_TYPE_WDS].Minor >= 9); +} // QCMAIN_IsDualIPSupported +#endif + +int requestSetupDataCall(PROFILE_T *profile, int curIpFamily) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err = 0; + UCHAR QMIType = (curIpFamily == IpFamilyV4) ? QMUX_TYPE_WDS : QMUX_TYPE_WDS_IPV6; + +//DualIPSupported means can get ipv4 & ipv6 address at the same time, one wds for ipv4, the other wds for ipv6 + profile->curIpFamily = curIpFamily; + pRequest = ComposeQMUXMsg(QMIType, QMIWDS_START_NETWORK_INTERFACE_REQ, WdsStartNwInterfaceReq, profile); + err = QmiThreadSendQMITimeout(pRequest, &pResponse, 120 * 1000); + qmi_rsp_check_and_return(); + + if (curIpFamily == IpFamilyV4) { + WdsConnectionIPv4Handle = le32_to_cpu(pResponse->MUXMsg.StartNwInterfaceResp.Handle); + dbg_time("%s WdsConnectionIPv4Handle: 0x%08x", __func__, WdsConnectionIPv4Handle); + } else { + WdsConnectionIPv6Handle = le32_to_cpu(pResponse->MUXMsg.StartNwInterfaceResp.Handle); + dbg_time("%s WdsConnectionIPv6Handle: 0x%08x", __func__, WdsConnectionIPv6Handle); + } + + free(pResponse); + + return 0; +} + +int requestDeactivateDefaultPDP(PROFILE_T *profile, int curIpFamily) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + UCHAR QMIType = (curIpFamily == 0x04) ? QMUX_TYPE_WDS : QMUX_TYPE_WDS_IPV6; + + if (curIpFamily == IpFamilyV4 && WdsConnectionIPv4Handle == 0) + return 0; + if (curIpFamily == IpFamilyV6 && WdsConnectionIPv6Handle == 0) + return 0; + + pRequest = ComposeQMUXMsg(QMIType, QMIWDS_STOP_NETWORK_INTERFACE_REQ , WdsStopNwInterfaceReq, &curIpFamily); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + if (curIpFamily == IpFamilyV4) + WdsConnectionIPv4Handle = 0; + else + WdsConnectionIPv6Handle = 0; + free(pResponse); + return 0; +} + +int requestGetIPAddress(PROFILE_T *profile, int curIpFamily) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR pIpv4Addr; + PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR pIpv6Addr = NULL; + PQMIWDS_GET_RUNTIME_SETTINGS_TLV_MTU pMtu; + IPV4_T *pIpv4 = &profile->ipv4; + IPV6_T *pIpv6 = &profile->ipv6; + UCHAR QMIType = (curIpFamily == 0x04) ? QMUX_TYPE_WDS : QMUX_TYPE_WDS_IPV6; + + if (curIpFamily == IpFamilyV4) { + memset(pIpv4, 0x00, sizeof(IPV4_T)); + if (WdsConnectionIPv4Handle == 0) + return 0; + } else if (curIpFamily == IpFamilyV6) { + memset(pIpv6, 0x00, sizeof(IPV6_T)); + if (WdsConnectionIPv6Handle == 0) + return 0; + } + + pRequest = ComposeQMUXMsg(QMIType, QMIWDS_GET_RUNTIME_SETTINGS_REQ, WdsGetRuntimeSettingReq, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + pIpv4Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4PRIMARYDNS); + if (pIpv4Addr) { + pIpv4->DnsPrimary = pIpv4Addr->IPV4Address; + } + + pIpv4Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4SECONDARYDNS); + if (pIpv4Addr) { + pIpv4->DnsSecondary = pIpv4Addr->IPV4Address; + } + + pIpv4Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4GATEWAY); + if (pIpv4Addr) { + pIpv4->Gateway = pIpv4Addr->IPV4Address; + } + + pIpv4Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4SUBNET); + if (pIpv4Addr) { + pIpv4->SubnetMask = pIpv4Addr->IPV4Address; + } + + pIpv4Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV4_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV4); + if (pIpv4Addr) { + pIpv4->Address = pIpv4Addr->IPV4Address; + } + + pIpv6Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6PRIMARYDNS); + if (pIpv6Addr) { + memcpy(pIpv6->DnsPrimary, pIpv6Addr->IPV6Address, 16); + } + + pIpv6Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6SECONDARYDNS); + if (pIpv6Addr) { + memcpy(pIpv6->DnsSecondary, pIpv6Addr->IPV6Address, 16); + } + + pIpv6Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6GATEWAY); + if (pIpv6Addr) { + memcpy(pIpv6->Gateway, pIpv6Addr->IPV6Address, 16); + pIpv6->PrefixLengthGateway = pIpv6Addr->PrefixLength; + } + + pIpv6Addr = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_IPV6_ADDR)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_IPV6); + if (pIpv6Addr) { + memcpy(pIpv6->Address, pIpv6Addr->IPV6Address, 16); + pIpv6->PrefixLengthIPAddr = pIpv6Addr->PrefixLength; + } + + pMtu = (PQMIWDS_GET_RUNTIME_SETTINGS_TLV_MTU)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, QMIWDS_GET_RUNTIME_SETTINGS_TLV_TYPE_MTU); + if (pMtu) { + pIpv4->Mtu = pIpv6->Mtu = le32_to_cpu(pMtu->Mtu); + } + + free(pResponse); + return 0; +} + +#ifdef CONFIG_APN +int requestSetProfile(PROFILE_T *profile) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + + if (!profile->pdp) + return 0; + + dbg_time("%s[%d] %s/%s/%s/%d", __func__, profile->pdp, profile->apn, profile->user, profile->password, profile->auth); + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS, QMIWDS_MODIFY_PROFILE_SETTINGS_REQ, WdsModifyProfileSettingsReq, profile); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + free(pResponse); + return 0; +} + +int requestGetProfile(PROFILE_T *profile) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + char *apn = NULL; + char *user = NULL; + char *password = NULL; + int auth = 0; + PQMIWDS_APNNAME pApnName; + PQMIWDS_USERNAME pUserName; + PQMIWDS_PASSWD pPassWd; + PQMIWDS_AUTH_PREFERENCE pAuthPref; + + if (!profile->pdp) + return 0; + + pRequest = ComposeQMUXMsg(QMUX_TYPE_WDS, QMIWDS_GET_PROFILE_SETTINGS_REQ, WdsGetProfileSettingsReqSend, profile); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + pApnName = (PQMIWDS_APNNAME)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x14); + pUserName = (PQMIWDS_USERNAME)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x1B); + pPassWd = (PQMIWDS_PASSWD)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x1C); + pAuthPref = (PQMIWDS_AUTH_PREFERENCE)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x1D); + + if (pApnName/* && le16_to_cpu(pApnName->TLVLength)*/) + apn = strndup((const char *)(&pApnName->ApnName), le16_to_cpu(pApnName->TLVLength)); + if (pUserName/* && pUserName->UserName*/) + user = strndup((const char *)(&pUserName->UserName), le16_to_cpu(pUserName->TLVLength)); + if (pPassWd/* && le16_to_cpu(pPassWd->TLVLength)*/) + password = strndup((const char *)(&pPassWd->Passwd), le16_to_cpu(pPassWd->TLVLength)); + if (pAuthPref/* && le16_to_cpu(pAuthPref->TLVLength)*/) { + auth = pAuthPref->AuthPreference; + } + +#if 0 + if (profile) { + profile->apn = apn; + profile->user = user; + profile->password = password; + profile->auth = auth; + } +#endif + + dbg_time("%s[%d] %s/%s/%s/%d", __func__, profile->pdp, apn, user, password, auth); + + free(pResponse); + return 0; +} +#endif + +#ifdef CONFIG_VERSION +int requestBaseBandVersion(const char **pp_reversion) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + PDEVICE_REV_ID revId; + int err; + + if (pp_reversion) *pp_reversion = NULL; + + pRequest = ComposeQMUXMsg(QMUX_TYPE_DMS, QMIDMS_GET_DEVICE_REV_ID_REQ, NULL, NULL); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + revId = (PDEVICE_REV_ID)GetTLV(&pResponse->MUXMsg.QMUXMsgHdr, 0x01); + + if (revId && le16_to_cpu(revId->TLVLength)) + { + char *DeviceRevisionID = strndup((const char *)(&revId->RevisionID), le16_to_cpu(revId->TLVLength)); + dbg_time("%s %s", __func__, DeviceRevisionID); + if (s_9x07 == -1) { //fail to get QMUX_TYPE_WDS_ADMIN + if (strncmp(DeviceRevisionID, "EC20", strlen("EC20"))) + s_9x07 = 1; + else + s_9x07 = DeviceRevisionID[5] == 'F' || DeviceRevisionID[6] == 'F'; //EC20CF,EC20EF,EC20CEF + } + if (pp_reversion) *pp_reversion = DeviceRevisionID; + } + + free(pResponse); + return 0; +} +#endif + +#ifdef CONFIG_RESET_RADIO +static USHORT DmsSetOperatingModeReq(PQMUX_MSG pMUXMsg, void *arg) { + pMUXMsg->SetOperatingModeReq.TLVType = 0x01; + pMUXMsg->SetOperatingModeReq.TLVLength = cpu_to_le16(1); + pMUXMsg->SetOperatingModeReq.OperatingMode = *((UCHAR *)arg); + + return sizeof(QMIDMS_SET_OPERATING_MODE_REQ_MSG); +} + +int requestSetOperatingMode(UCHAR OperatingMode) { + PQCQMIMSG pRequest; + PQCQMIMSG pResponse; + PQMUX_MSG pMUXMsg; + int err; + + dbg_time("%s(%d)", __func__, OperatingMode); + + pRequest = ComposeQMUXMsg(QMUX_TYPE_DMS, QMIDMS_SET_OPERATING_MODE_REQ, DmsSetOperatingModeReq, &OperatingMode); + err = QmiThreadSendQMI(pRequest, &pResponse); + qmi_rsp_check_and_return(); + + free(pResponse); + return 0; +} +#endif diff --git a/quectel-CM/QMIThread.h b/quectel-CM/QMIThread.h new file mode 100755 index 0000000..bcd8672 --- /dev/null +++ b/quectel-CM/QMIThread.h @@ -0,0 +1,177 @@ +#ifndef __QMI_THREAD_H__ +#define __QMI_THREAD_H__ + +#define CONFIG_GOBINET +#define CONFIG_QMIWWAN +#define CONFIG_SIM +#define CONFIG_APN +#define CONFIG_VERSION +#define CONFIG_DEFAULT_PDP 1 +//#define CONFIG_IMSI_ICCID +#ifndef ANDROID +#define CONFIG_RESET_RADIO (45) //Reset Radiao(AT+CFUN=4,AT+CFUN=1) when cann not register network or setup data call in 45 seconds +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MPQMI.h" +#include "MPQCTL.h" +#include "MPQMUX.h" + +#define DEVICE_CLASS_UNKNOWN 0 +#define DEVICE_CLASS_CDMA 1 +#define DEVICE_CLASS_GSM 2 + +#define WWAN_DATA_CLASS_NONE 0x00000000 +#define WWAN_DATA_CLASS_GPRS 0x00000001 +#define WWAN_DATA_CLASS_EDGE 0x00000002 /* EGPRS */ +#define WWAN_DATA_CLASS_UMTS 0x00000004 +#define WWAN_DATA_CLASS_HSDPA 0x00000008 +#define WWAN_DATA_CLASS_HSUPA 0x00000010 +#define WWAN_DATA_CLASS_LTE 0x00000020 +#define WWAN_DATA_CLASS_1XRTT 0x00010000 +#define WWAN_DATA_CLASS_1XEVDO 0x00020000 +#define WWAN_DATA_CLASS_1XEVDO_REVA 0x00040000 +#define WWAN_DATA_CLASS_1XEVDV 0x00080000 +#define WWAN_DATA_CLASS_3XRTT 0x00100000 +#define WWAN_DATA_CLASS_1XEVDO_REVB 0x00200000 /* for future use */ +#define WWAN_DATA_CLASS_UMB 0x00400000 +#define WWAN_DATA_CLASS_CUSTOM 0x80000000 + +struct wwan_data_class_str { + ULONG class; + CHAR *str; +}; + +#pragma pack(push, 1) + +typedef struct _QCQMIMSG { + QCQMI_HDR QMIHdr; + union { + QMICTL_MSG CTLMsg; + QMUX_MSG MUXMsg; + }; +} __attribute__ ((packed)) QCQMIMSG, *PQCQMIMSG; + +#pragma pack(pop) + +typedef struct __IPV4 { + uint32_t Address; + uint32_t Gateway; + uint32_t SubnetMask; + uint32_t DnsPrimary; + uint32_t DnsSecondary; + uint32_t Mtu; +} IPV4_T; + +typedef struct __IPV6 { + UCHAR Address[16]; + UCHAR Gateway[16]; + UCHAR SubnetMask[16]; + UCHAR DnsPrimary[16]; + UCHAR DnsSecondary[16]; + UCHAR PrefixLengthIPAddr; + UCHAR PrefixLengthGateway; + ULONG Mtu; +} IPV6_T; + +#define IpFamilyV4 (0x04) +#define IpFamilyV6 (0x06) +typedef struct __PROFILE { + char * qmichannel; + char * usbnet_adapter; + char * qmapnet_adapter; + int qmap_mode; + int qmap_version; + const char *apn; + const char *user; + const char *password; + const char *pincode; + int auth; + int pdp; + int IsDualIPSupported; + int curIpFamily; + int rawIP; + IPV4_T ipv4; + IPV6_T ipv6; +} PROFILE_T; + +typedef enum { + SIM_ABSENT = 0, + SIM_NOT_READY = 1, + SIM_READY = 2, /* SIM_READY means the radio state is RADIO_STATE_SIM_READY */ + SIM_PIN = 3, + SIM_PUK = 4, + SIM_NETWORK_PERSONALIZATION = 5, + SIM_BAD = 6, +} SIM_Status; + +#define WDM_DEFAULT_BUFSIZE 256 +#define RIL_REQUEST_QUIT 0x1000 +#define RIL_INDICATE_DEVICE_CONNECTED 0x1002 +#define RIL_INDICATE_DEVICE_DISCONNECTED 0x1003 +#define RIL_UNSOL_RESPONSE_VOICE_NETWORK_STATE_CHANGED 0x1004 +#define RIL_UNSOL_DATA_CALL_LIST_CHANGED 0x1005 + +extern int pthread_cond_timeout_np(pthread_cond_t *cond, pthread_mutex_t * mutex, unsigned msecs); +extern int QmiThreadSendQMI(PQCQMIMSG pRequest, PQCQMIMSG *ppResponse); +extern int QmiThreadSendQMITimeout(PQCQMIMSG pRequest, PQCQMIMSG *ppResponse, unsigned msecs); +extern void QmiThreadRecvQMI(PQCQMIMSG pResponse); +extern int QmiWwanInit(PROFILE_T *profile); +extern int QmiWwanDeInit(void); +extern int QmiWwanSendQMI(PQCQMIMSG pRequest); +extern void * QmiWwanThread(void *pData); +extern int GobiNetSendQMI(PQCQMIMSG pRequest); +extern void * GobiNetThread(void *pData); +extern void udhcpc_start(PROFILE_T *profile); +extern void udhcpc_stop(PROFILE_T *profile); +extern void dump_qmi(void *dataBuffer, int dataLen); +extern void qmidevice_send_event_to_main(int triger_event); +extern int requestSetEthMode(PROFILE_T *profile); +extern int requestGetSIMStatus(SIM_Status *pSIMStatus); +extern int requestEnterSimPin(const CHAR *pPinCode); +extern int requestGetICCID(void); +extern int requestGetIMSI(void); +extern int requestRegistrationState(UCHAR *pPSAttachedState); +extern int requestQueryDataCall(UCHAR *pConnectionStatus, int curIpFamily); +extern int requestSetupDataCall(PROFILE_T *profile, int curIpFamily); +extern int requestDeactivateDefaultPDP(PROFILE_T *profile, int curIpFamily); +extern int requestSetProfile(PROFILE_T *profile); +extern int requestGetProfile(PROFILE_T *profile); +extern int requestBaseBandVersion(const char **pp_reversion); +extern int requestGetIPAddress(PROFILE_T *profile, int curIpFamily); +extern int requestSetOperatingMode(UCHAR OperatingMode); + +extern FILE *logfilefp; +extern int debug_qmi; +extern char * qmichannel; +extern int qmidevice_control_fd[2]; +extern int qmiclientId[QMUX_TYPE_WDS_ADMIN + 1]; +extern int cdc_wdm_fd; +extern void dbg_time (const char *fmt, ...); +extern USHORT le16_to_cpu(USHORT v16); +extern UINT le32_to_cpu (UINT v32); +extern UINT ql_swap32(UINT v32); +extern USHORT cpu_to_le16(USHORT v16); +extern UINT cpu_to_le32(UINT v32); +#endif diff --git a/quectel-CM/QmiWwanCM.c b/quectel-CM/QmiWwanCM.c new file mode 100755 index 0000000..2587234 --- /dev/null +++ b/quectel-CM/QmiWwanCM.c @@ -0,0 +1,359 @@ +#include +#include +#include +#include +#include +typedef unsigned short sa_family_t; +#include +#include "QMIThread.h" + +#ifdef CONFIG_QMIWWAN +int cdc_wdm_fd = -1; +static UCHAR GetQCTLTransactionId(void) { + static int TransactionId = 0; + if (++TransactionId > 0xFF) + TransactionId = 1; + return TransactionId; +} + +typedef USHORT (*CUSTOMQCTL)(PQMICTL_MSG pCTLMsg, void *arg); + +static PQCQMIMSG ComposeQCTLMsg(USHORT QMICTLType, CUSTOMQCTL customQctlMsgFunction, void *arg) { + UCHAR QMIBuf[WDM_DEFAULT_BUFSIZE]; + PQCQMIMSG pRequest = (PQCQMIMSG)QMIBuf; + int Length; + + pRequest->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; + pRequest->QMIHdr.CtlFlags = 0x00; + pRequest->QMIHdr.QMIType = QMUX_TYPE_CTL; + pRequest->QMIHdr.ClientId= 0x00; + + pRequest->CTLMsg.QMICTLMsgHdr.CtlFlags = QMICTL_FLAG_REQUEST; + pRequest->CTLMsg.QMICTLMsgHdr.TransactionId = GetQCTLTransactionId(); + pRequest->CTLMsg.QMICTLMsgHdr.QMICTLType = cpu_to_le16(QMICTLType); + if (customQctlMsgFunction) + pRequest->CTLMsg.QMICTLMsgHdr.Length = cpu_to_le16(customQctlMsgFunction(&pRequest->CTLMsg, arg) - sizeof(QCQMICTL_MSG_HDR)); + else + pRequest->CTLMsg.QMICTLMsgHdr.Length = cpu_to_le16(0x0000); + + pRequest->QMIHdr.Length = cpu_to_le16(le16_to_cpu(pRequest->CTLMsg.QMICTLMsgHdr.Length) + sizeof(QCQMICTL_MSG_HDR) + sizeof(QCQMI_HDR) - 1); + Length = le16_to_cpu(pRequest->QMIHdr.Length) + 1; + + pRequest = (PQCQMIMSG)malloc(Length); + if (pRequest == NULL) { + dbg_time("%s fail to malloc", __func__); + } else { + memcpy(pRequest, QMIBuf, Length); + } + + return pRequest; +} + +static USHORT CtlGetVersionReq(PQMICTL_MSG QCTLMsg, void *arg) { + QCTLMsg->GetVersionReq.TLVType = QCTLV_TYPE_REQUIRED_PARAMETER; + QCTLMsg->GetVersionReq.TLVLength = cpu_to_le16(0x0001); + QCTLMsg->GetVersionReq.QMUXTypes = QMUX_TYPE_ALL; + return sizeof(QMICTL_GET_VERSION_REQ_MSG); +} + +static USHORT CtlGetClientIdReq(PQMICTL_MSG QCTLMsg, void *arg) { + QCTLMsg->GetClientIdReq.TLVType = QCTLV_TYPE_REQUIRED_PARAMETER; + QCTLMsg->GetClientIdReq.TLVLength = cpu_to_le16(0x0001); + QCTLMsg->GetClientIdReq.QMIType = ((UCHAR *)arg)[0]; + return sizeof(QMICTL_GET_CLIENT_ID_REQ_MSG); +} + +static USHORT CtlReleaseClientIdReq(PQMICTL_MSG QCTLMsg, void *arg) { + QCTLMsg->ReleaseClientIdReq.TLVType = QCTLV_TYPE_REQUIRED_PARAMETER; + QCTLMsg->ReleaseClientIdReq.TLVLength = cpu_to_le16(0x0002); + QCTLMsg->ReleaseClientIdReq.QMIType = ((UCHAR *)arg)[0]; + QCTLMsg->ReleaseClientIdReq.ClientId = ((UCHAR *)arg)[1] ; + return sizeof(QMICTL_RELEASE_CLIENT_ID_REQ_MSG); +} + +int QmiWwanSendQMI(PQCQMIMSG pRequest) { + struct pollfd pollfds[]= {{cdc_wdm_fd, POLLOUT, 0}}; + int ret; + + if (cdc_wdm_fd == -1) { + dbg_time("%s cdc_wdm_fd = -1", __func__); + return -ENODEV; + } + + if (pRequest->QMIHdr.QMIType == QMUX_TYPE_WDS_IPV6) + pRequest->QMIHdr.QMIType = QMUX_TYPE_WDS; + + do { + ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000); + } while ((ret < 0) && (errno == EINTR)); + + if (pollfds[0].revents & POLLOUT) { + ssize_t nwrites = le16_to_cpu(pRequest->QMIHdr.Length) + 1; + ret = write(cdc_wdm_fd, pRequest, nwrites); + if (ret == nwrites) { + ret = 0; + } else { + dbg_time("%s write=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + } + } else { + dbg_time("%s poll=%d, revents = 0x%x, errno: %d (%s)", __func__, ret, pollfds[0].revents, errno, strerror(errno)); + } + + return ret; +} + +static int QmiWwanGetClientID(UCHAR QMIType) { + PQCQMIMSG pResponse; + + QmiThreadSendQMI(ComposeQCTLMsg(QMICTL_GET_CLIENT_ID_REQ, CtlGetClientIdReq, &QMIType), &pResponse); + + if (pResponse) { + USHORT QMUXResult = cpu_to_le16(pResponse->CTLMsg.QMICTLMsgHdrRsp.QMUXResult); // QMI_RESULT_SUCCESS + USHORT QMUXError = cpu_to_le16(pResponse->CTLMsg.QMICTLMsgHdrRsp.QMUXError); // QMI_ERR_INVALID_ARG + //UCHAR QMIType = pResponse->CTLMsg.GetClientIdRsp.QMIType; + UCHAR ClientId = pResponse->CTLMsg.GetClientIdRsp.ClientId; + + if (!QMUXResult && !QMUXError && (QMIType == pResponse->CTLMsg.GetClientIdRsp.QMIType)) { + switch (QMIType) { + case QMUX_TYPE_WDS: dbg_time("Get clientWDS = %d", ClientId); break; + case QMUX_TYPE_DMS: dbg_time("Get clientDMS = %d", ClientId); break; + case QMUX_TYPE_NAS: dbg_time("Get clientNAS = %d", ClientId); break; + case QMUX_TYPE_QOS: dbg_time("Get clientQOS = %d", ClientId); break; + case QMUX_TYPE_WMS: dbg_time("Get clientWMS = %d", ClientId); break; + case QMUX_TYPE_PDS: dbg_time("Get clientPDS = %d", ClientId); break; + case QMUX_TYPE_UIM: dbg_time("Get clientUIM = %d", ClientId); break; + case QMUX_TYPE_WDS_ADMIN: dbg_time("Get clientWDA = %d", ClientId); + break; + default: break; + } + return ClientId; + } + } + return 0; +} + +static int QmiWwanReleaseClientID(QMI_SERVICE_TYPE QMIType, UCHAR ClientId) { + UCHAR argv[] = {QMIType, ClientId}; + QmiThreadSendQMI(ComposeQCTLMsg(QMICTL_RELEASE_CLIENT_ID_REQ, CtlReleaseClientIdReq, argv), NULL); + return 0; +} + +int QmiWwanInit(PROFILE_T *profile) { + unsigned i; + int ret; + PQCQMIMSG pResponse; + + if (profile->qmapnet_adapter == NULL || profile->qmapnet_adapter == profile->usbnet_adapter) + { + for (i = 0; i < 10; i++) { + ret = QmiThreadSendQMITimeout(ComposeQCTLMsg(QMICTL_SYNC_REQ, NULL, NULL), NULL, 1 * 1000); + if (!ret) + break; + sleep(1); + } + if (ret) + return ret; + } + + QmiThreadSendQMI(ComposeQCTLMsg(QMICTL_GET_VERSION_REQ, CtlGetVersionReq, NULL), &pResponse); + if (profile->qmapnet_adapter != NULL && profile->qmapnet_adapter == profile->usbnet_adapter) { + if (pResponse) { + if (pResponse->CTLMsg.QMICTLMsgHdrRsp.QMUXResult == 0 && pResponse->CTLMsg.QMICTLMsgHdrRsp.QMUXError == 0) { + uint8_t NumElements = 0; + + for (NumElements = 0; NumElements < pResponse->CTLMsg.GetVersionRsp.NumElements; NumElements++) { +#if 0 + dbg_time("QMUXType = %02x Version = %d.%d", + pResponse->CTLMsg.GetVersionRsp.TypeVersion[NumElements].QMUXType, + pResponse->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MajorVersion, + pResponse->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MinorVersion); +#endif + if (pResponse->CTLMsg.GetVersionRsp.TypeVersion[NumElements].QMUXType == QMUX_TYPE_WDS_ADMIN) + profile->qmap_version = (pResponse->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MinorVersion > 16); + } + } + } + } + if (pResponse) free(pResponse); + qmiclientId[QMUX_TYPE_WDS] = QmiWwanGetClientID(QMUX_TYPE_WDS); + if (profile->IsDualIPSupported) + qmiclientId[QMUX_TYPE_WDS_IPV6] = QmiWwanGetClientID(QMUX_TYPE_WDS); + qmiclientId[QMUX_TYPE_DMS] = QmiWwanGetClientID(QMUX_TYPE_DMS); + qmiclientId[QMUX_TYPE_NAS] = QmiWwanGetClientID(QMUX_TYPE_NAS); + qmiclientId[QMUX_TYPE_UIM] = QmiWwanGetClientID(QMUX_TYPE_UIM); + if (profile->qmapnet_adapter == NULL || profile->qmapnet_adapter == profile->usbnet_adapter) + qmiclientId[QMUX_TYPE_WDS_ADMIN] = QmiWwanGetClientID(QMUX_TYPE_WDS_ADMIN); + + return 0; +} + +int QmiWwanDeInit(void) { + unsigned int i; + for (i = 0; i < sizeof(qmiclientId)/sizeof(qmiclientId[0]); i++) + { + if (qmiclientId[i] != 0) + { + QmiWwanReleaseClientID(i, qmiclientId[i]); + qmiclientId[i] = 0; + } + } + + return 0; +} + +#define QUECTEL_QMI_PROXY "quectel-qmi-proxy" +static int qmi_proxy_open(const char *name) { + int sockfd = -1; + int reuse_addr = 1; + struct sockaddr_un sockaddr; + socklen_t alen; + + /*Create server socket*/ + (sockfd = socket(AF_LOCAL, SOCK_STREAM, 0)); + if (sockfd < 0) + return sockfd; + + memset(&sockaddr, 0, sizeof(sockaddr)); + sockaddr.sun_family = AF_LOCAL; + sockaddr.sun_path[0] = 0; + memcpy(sockaddr.sun_path + 1, name, strlen(name) ); + + alen = strlen(name) + offsetof(struct sockaddr_un, sun_path) + 1; + if(connect(sockfd, (struct sockaddr *)&sockaddr, alen) < 0) { + close(sockfd); + dbg_time("%s connect %s errno: %d (%s)\n", __func__, name, errno, strerror(errno)); + return -1; + } + (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &reuse_addr,sizeof(reuse_addr))); + + dbg_time("connect to %s sockfd = %d\n", name, sockfd); + + return sockfd; +} + +static ssize_t qmi_proxy_read (int fd, void *buf, size_t size) { + ssize_t nreads; + PQCQMI_HDR pHdr = (PQCQMI_HDR)buf; + + nreads = read(fd, pHdr, sizeof(QCQMI_HDR)); + if (nreads == sizeof(QCQMI_HDR)) { + nreads += read(fd, pHdr+1, le16_to_cpu(pHdr->Length) + 1 - sizeof(QCQMI_HDR)); + } + + return nreads; +} + +void * QmiWwanThread(void *pData) { + PROFILE_T *profile = (PROFILE_T *)pData; + const char *cdc_wdm = (const char *)profile->qmichannel; + + if (profile->qmapnet_adapter == NULL || profile->qmapnet_adapter == profile->usbnet_adapter) + cdc_wdm_fd = open(cdc_wdm, O_RDWR | O_NONBLOCK | O_NOCTTY); + else + cdc_wdm_fd = qmi_proxy_open(QUECTEL_QMI_PROXY); + + if (cdc_wdm_fd == -1) { + dbg_time("%s Failed to open %s, errno: %d (%s)", __func__, cdc_wdm, errno, strerror(errno)); + qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_DISCONNECTED); + pthread_exit(NULL); + return NULL; + } + + fcntl(cdc_wdm_fd, F_SETFL, fcntl(cdc_wdm_fd,F_GETFL) | O_NONBLOCK); + fcntl(cdc_wdm_fd, F_SETFD, FD_CLOEXEC); + + dbg_time("cdc_wdm_fd = %d", cdc_wdm_fd); + + qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_CONNECTED); + + while (1) { + struct pollfd pollfds[] = {{qmidevice_control_fd[1], POLLIN, 0}, {cdc_wdm_fd, POLLIN, 0}}; + int ne, ret, nevents = sizeof(pollfds)/sizeof(pollfds[0]); + + do { + ret = poll(pollfds, nevents, -1); + } while ((ret < 0) && (errno == EINTR)); + + if (ret <= 0) { + dbg_time("%s poll=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + break; + } + + for (ne = 0; ne < nevents; ne++) { + int fd = pollfds[ne].fd; + short revents = pollfds[ne].revents; + + //dbg_time("{%d, %x, %x}", pollfds[ne].fd, pollfds[ne].events, pollfds[ne].revents); + + if (revents & (POLLERR | POLLHUP | POLLNVAL)) { + dbg_time("%s poll err/hup/inval", __func__); + dbg_time("poll fd = %d, events = 0x%04x", fd, revents); + if (fd == cdc_wdm_fd) { + } else { + } + if (revents & (POLLHUP | POLLNVAL)) //EC20 bug, Can get POLLERR + goto __QmiWwanThread_quit; + } + + if ((revents & POLLIN) == 0) + continue; + + if (fd == qmidevice_control_fd[1]) { + int triger_event; + if (read(fd, &triger_event, sizeof(triger_event)) == sizeof(triger_event)) { + //DBG("triger_event = 0x%x", triger_event); + switch (triger_event) { + case RIL_REQUEST_QUIT: + goto __QmiWwanThread_quit; + break; + case SIGTERM: + case SIGHUP: + case SIGINT: + QmiThreadRecvQMI(NULL); + break; + default: + break; + } + } + } + + if (fd == cdc_wdm_fd) { + ssize_t nreads; + UCHAR QMIBuf[512]; + PQCQMIMSG pResponse = (PQCQMIMSG)QMIBuf; + + if (profile->qmapnet_adapter == NULL || profile->qmapnet_adapter == profile->usbnet_adapter) + nreads = read(fd, QMIBuf, sizeof(QMIBuf)); + else + nreads = qmi_proxy_read(fd, QMIBuf, sizeof(QMIBuf)); + //dbg_time("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno)); + if (nreads <= 0) { + dbg_time("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno)); + break; + } + + if (nreads != (le16_to_cpu(pResponse->QMIHdr.Length) + 1)) { + dbg_time("%s nreads=%d, pQCQMI->QMIHdr.Length = %d", __func__, (int)nreads, le16_to_cpu(pResponse->QMIHdr.Length)); + continue; + } + + QmiThreadRecvQMI(pResponse); + } + } + } + +__QmiWwanThread_quit: + if (cdc_wdm_fd != -1) { close(cdc_wdm_fd); cdc_wdm_fd = -1; } + qmidevice_send_event_to_main(RIL_INDICATE_DEVICE_DISCONNECTED); + QmiThreadRecvQMI(NULL); //main thread may pending on QmiThreadSendQMI() + dbg_time("%s exit", __func__); + pthread_exit(NULL); + return NULL; +} + +#else +int QmiWwanSendQMI(PQCQMIMSG pRequest) {return -1;} +int QmiWwanInit(PROFILE_T *profile) {return -1;} +int QmiWwanDeInit(void) {return -1;} +void * QmiWwanThread(void *pData) {dbg_time("please set CONFIG_QMIWWAN"); return NULL;} +#endif diff --git a/quectel-CM/README.md b/quectel-CM/README.md new file mode 100644 index 0000000..208a325 --- /dev/null +++ b/quectel-CM/README.md @@ -0,0 +1,6 @@ +# Quectel Connect Manager +For those who struggle to get the sources. Their support gave me the sources. +## Version 1.1.45 for Linux&Android +This is the version 1.1.45 of the **Connect Manager** from Quectel. +## Makefile +The Makefile was adapted to work with the Yocto/Poky build envrionnement (not much done but 'as is', it wasn't working). \ No newline at end of file diff --git a/quectel-CM/default.script b/quectel-CM/default.script new file mode 100755 index 0000000..ddce8d4 --- /dev/null +++ b/quectel-CM/default.script @@ -0,0 +1,63 @@ +#!/bin/sh +# Busybox udhcpc dispatcher script. Copyright (C) 2009 by Axel Beckert. +# +# Based on the busybox example scripts and the old udhcp source +# package default.* scripts. + +RESOLV_CONF="/etc/resolv.conf" + +case $1 in + bound|renew) + [ -n "$broadcast" ] && BROADCAST="broadcast $broadcast" + [ -n "$subnet" ] && NETMASK="netmask $subnet" + + /sbin/ifconfig $interface $ip $BROADCAST $NETMASK + + if [ -n "$router" ]; then + echo "$0: Resetting default routes" + while /sbin/route del default gw 0.0.0.0 dev $interface; do :; done + + metric=0 + for i in $router; do + /sbin/route add default gw $i dev $interface metric $metric + metric=$(($metric + 1)) + done + fi + + # Update resolver configuration file + R="" + [ -n "$domain" ] && R="domain $domain +" + for i in $dns; do + echo "$0: Adding DNS $i" + R="${R}nameserver $i +" + done + + if [ ! -x /sbin/resolvconf ]; then + echo -n "$R" | resolvconf -a "${interface}.udhcpc" + else + echo -n "$R" > "$RESOLV_CONF" + fi + ;; + + deconfig) + if [ -x /sbin/resolvconf ]; then + resolvconf -d "${interface}.udhcpc" + fi + /sbin/ifconfig $interface 0.0.0.0 + ;; + + leasefail) + echo "$0: Lease failed: $message" + ;; + + nak) + echo "$0: Received a NAK: $message" + ;; + + *) + echo "$0: Unknown udhcpc command: $1"; + exit 1; + ;; +esac diff --git a/quectel-CM/dhcpclient.c b/quectel-CM/dhcpclient.c new file mode 100755 index 0000000..1b669c2 --- /dev/null +++ b/quectel-CM/dhcpclient.c @@ -0,0 +1,90 @@ +#ifdef ANDROID +/* + * Copyright 2008, The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "QMIThread.h" +#ifdef USE_NDK +extern int (*ifc_init)(void); +extern void (*ifc_close)(void); +extern int (*do_dhcp)(const char *iname); +extern void (*get_dhcp_info)(uint32_t *ipaddr, uint32_t *gateway, uint32_t *prefixLength, + uint32_t *dns1, uint32_t *dns2, uint32_t *server, + uint32_t *lease); +extern int (*property_set)(const char *key, const char *value); +#else +#include +#include +extern int do_dhcp(const char *iname); +extern void get_dhcp_info(uint32_t *ipaddr, uint32_t *gateway, uint32_t *prefixLength, + uint32_t *dns1, uint32_t *dns2, uint32_t *server, + uint32_t *lease); +#endif + +static const char *ipaddr_to_string(in_addr_t addr) +{ + struct in_addr in_addr; + + in_addr.s_addr = addr; + return inet_ntoa(in_addr); +} + +void do_dhcp_request(PROFILE_T *profile) { +#ifdef USE_NDK + if (!ifc_init ||!ifc_close ||!do_dhcp || !get_dhcp_info || !property_set) { + return; + } +#endif + + char *ifname = profile->usbnet_adapter; + uint32_t ipaddr, gateway, prefixLength, dns1, dns2, server, lease; + char propKey[128]; + +#if 0 + if (profile->rawIP && ((profile->IPType==0x04 && profile->ipv4.Address))) + { + snprintf(propKey, sizeof(propKey), "net.%s.dns1", ifname); + property_set(propKey, profile->ipv4.DnsPrimary ? ipaddr_to_string(ql_swap32(profile->ipv4.DnsPrimary)) : "8.8.8.8"); + snprintf(propKey, sizeof(propKey), "net.%s.dns2", ifname); + property_set(propKey, profile->ipv4.DnsSecondary ? ipaddr_to_string(ql_swap32(profile->ipv4.DnsSecondary)) : "8.8.8.8"); + snprintf(propKey, sizeof(propKey), "net.%s.gw", ifname); + property_set(propKey, profile->ipv4.Gateway ? ipaddr_to_string(ql_swap32(profile->ipv4.Gateway)) : "0.0.0.0"); + return; + } +#endif + + if(ifc_init()) { + dbg_time("failed to ifc_init(%s): %s\n", ifname, strerror(errno)); + } + + if (do_dhcp(ifname) < 0) { + dbg_time("failed to do_dhcp(%s): %s\n", ifname, strerror(errno)); + } + + ifc_close(); + + get_dhcp_info(&ipaddr, &gateway, &prefixLength, &dns1, &dns2, &server, &lease); + snprintf(propKey, sizeof(propKey), "net.%s.gw", ifname); + property_set(propKey, gateway ? ipaddr_to_string(gateway) : "0.0.0.0"); +} +#endif diff --git a/quectel-CM/main.c b/quectel-CM/main.c new file mode 100755 index 0000000..60fc301 --- /dev/null +++ b/quectel-CM/main.c @@ -0,0 +1,1080 @@ +#include "QMIThread.h" +#include +#include +#include +#include + +//#define CONFIG_EXIT_WHEN_DIAL_FAILED +//#define CONFIG_BACKGROUND_WHEN_GET_IP +//#define CONFIG_PID_FILE_FORMAT "/var/run/quectel-CM-%s.pid" //for example /var/run/quectel-CM-wwan0.pid + +int debug_qmi = 0; +int main_loop = 0; +char * qmichannel; +int qmidevice_control_fd[2]; +static int signal_control_fd[2]; + +#ifdef CONFIG_BACKGROUND_WHEN_GET_IP +static int daemon_pipe_fd[2]; + +static void ql_prepare_daemon(void) { + pid_t daemon_child_pid; + + if (pipe(daemon_pipe_fd) < 0) { + dbg_time("%s Faild to create daemon_pipe_fd: %d (%s)", __func__, errno, strerror(errno)); + return; + } + + daemon_child_pid = fork(); + if (daemon_child_pid > 0) { + struct pollfd pollfds[] = {{daemon_pipe_fd[0], POLLIN, 0}, {0, POLLIN, 0}}; + int ne, ret, nevents = sizeof(pollfds)/sizeof(pollfds[0]); + int signo; + + //dbg_time("father"); + + close(daemon_pipe_fd[1]); + + if (socketpair( AF_LOCAL, SOCK_STREAM, 0, signal_control_fd) < 0 ) { + dbg_time("%s Faild to create main_control_fd: %d (%s)", __func__, errno, strerror(errno)); + return; + } + + pollfds[1].fd = signal_control_fd[1]; + + while (1) { + do { + ret = poll(pollfds, nevents, -1); + } while ((ret < 0) && (errno == EINTR)); + + if (ret < 0) { + dbg_time("%s poll=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + goto __daemon_quit; + } + + for (ne = 0; ne < nevents; ne++) { + int fd = pollfds[ne].fd; + short revents = pollfds[ne].revents; + + if (revents & (POLLERR | POLLHUP | POLLNVAL)) { + //dbg_time("%s poll err/hup", __func__); + //dbg_time("poll fd = %d, events = 0x%04x", fd, revents); + if (revents & POLLHUP) + goto __daemon_quit; + } + + if ((revents & POLLIN) && read(fd, &signo, sizeof(signo)) == sizeof(signo)) { + if (signal_control_fd[1] == fd) { + if (signo == SIGCHLD) { + int status; + int pid = waitpid(daemon_child_pid, &status, 0); + dbg_time("waitpid pid=%d, status=%x", pid, status); + goto __daemon_quit; + } else { + kill(daemon_child_pid, signo); + } + } else if (daemon_pipe_fd[0] == fd) { + //dbg_time("daemon_pipe_signo = %d", signo); + goto __daemon_quit; + } + } + } + } +__daemon_quit: + //dbg_time("father exit"); + _exit(0); + } else if (daemon_child_pid == 0) { + close(daemon_pipe_fd[0]); + //dbg_time("child", getpid()); + } else { + close(daemon_pipe_fd[0]); + close(daemon_pipe_fd[1]); + dbg_time("%s Faild to create daemon_child_pid: %d (%s)", __func__, errno, strerror(errno)); + } +} + +static void ql_enter_daemon(int signo) { + if (daemon_pipe_fd[1] > 0) + if (signo) { + write(daemon_pipe_fd[1], &signo, sizeof(signo)); + sleep(1); + } + close(daemon_pipe_fd[1]); + daemon_pipe_fd[1] = -1; + setsid(); + } +#endif + +//UINT ifc_get_addr(const char *ifname); + +static void usbnet_link_change(int link, PROFILE_T *profile) { + static int s_link = 0; + + if (s_link == link) + return; + + s_link = link; + + if (link) { + requestGetIPAddress(profile, IpFamilyV4); + if (profile->IsDualIPSupported) + requestGetIPAddress(profile, IpFamilyV6); + udhcpc_start(profile); + } else { + udhcpc_stop(profile); + } + +#ifdef LINUX_RIL_SHLIB + if (link) { + int timeout = 6; + while (timeout-- /*&& ifc_get_addr(profile->usbnet_adapter) == 0*/) { + sleep(1); + } + } + + if (link && requestGetIPAddress(profile, 0x04) == 0) { + unsigned char *r; + + dbg_time("Using interface %s", profile->usbnet_adapter); + r = (unsigned char *)&profile->ipv4.Address; + dbg_time("local IP address %d.%d.%d.%d", r[3], r[2], r[1], r[0]); + r = (unsigned char *)&profile->ipv4.Gateway; + dbg_time("remote IP address %d.%d.%d.%d", r[3], r[2], r[1], r[0]); + r = (unsigned char *)&profile->ipv4.DnsPrimary; + dbg_time("primary DNS address %d.%d.%d.%d", r[3], r[2], r[1], r[0]); + r = (unsigned char *)&profile->ipv4.DnsSecondary; + dbg_time("secondary DNS address %d.%d.%d.%d", r[3], r[2], r[1], r[0]); + } +#endif + +#ifdef CONFIG_BACKGROUND_WHEN_GET_IP + if (link && daemon_pipe_fd[1] > 0) { + int timeout = 6; + while (timeout-- /*&& ifc_get_addr(profile->usbnet_adapter) == 0*/) { + sleep(1); + } + ql_enter_daemon(SIGUSR1); + } +#endif +} + +static int check_ipv4_address(PROFILE_T *now_profile) { + PROFILE_T new_profile_v; + PROFILE_T *new_profile = &new_profile_v; + + memcpy(new_profile, now_profile, sizeof(PROFILE_T)); + if (requestGetIPAddress(new_profile, 0x04) == 0) { + if (new_profile->ipv4.Address != now_profile->ipv4.Address || debug_qmi) { + unsigned char *l = (unsigned char *)&now_profile->ipv4.Address; + unsigned char *r = (unsigned char *)&new_profile->ipv4.Address; + dbg_time("localIP: %d.%d.%d.%d VS remoteIP: %d.%d.%d.%d", + l[3], l[2], l[1], l[0], r[3], r[2], r[1], r[0]); + } + return (new_profile->ipv4.Address == now_profile->ipv4.Address); + } + return 0; +} + +static void main_send_event_to_qmidevice(int triger_event) { + write(qmidevice_control_fd[0], &triger_event, sizeof(triger_event)); +} + +static void send_signo_to_main(int signo) { + write(signal_control_fd[0], &signo, sizeof(signo)); +} + +void qmidevice_send_event_to_main(int triger_event) { + write(qmidevice_control_fd[1], &triger_event, sizeof(triger_event)); +} + +#define MAX_PATH 256 + +static int ls_dir(const char *dir, int (*match)(const char *dir, const char *file, void *argv[]), void *argv[]) +{ + DIR *pDir; + struct dirent* ent = NULL; + int match_times = 0; + + pDir = opendir(dir); + if (pDir == NULL) { + dbg_time("Cannot open directory: %s, errno: %d (%s)", dir, errno, strerror(errno)); + return 0; + } + + while ((ent = readdir(pDir)) != NULL) { + match_times += match(dir, ent->d_name, argv); + } + closedir(pDir); + + return match_times; +} + +static int is_same_linkfile(const char *dir, const char *file, void *argv[]) +{ + const char *qmichannel = (const char *)argv[1]; + char linkname[MAX_PATH]; + char filename[MAX_PATH]; + int linksize; + + snprintf(linkname, MAX_PATH, "%s/%s", dir, file); + linksize = readlink(linkname, filename, MAX_PATH); + if (linksize <= 0) + return 0; + + filename[linksize] = 0; + if (strcmp(filename, qmichannel)) + return 0; + + dbg_time("%s -> %s", linkname, filename); + return 1; +} + +static int is_brother_process(const char *dir, const char *file, void *argv[]) +{ + //const char *myself = (const char *)argv[0]; + char linkname[MAX_PATH]; + char filename[MAX_PATH]; + int linksize; + int i = 0, kill_timeout = 15; + pid_t pid; + + //dbg_time("%s", file); + while (file[i]) { + if (!isdigit(file[i])) + break; + i++; + } + + if (file[i]) { + //dbg_time("%s not digit", file); + return 0; + } + + snprintf(linkname, MAX_PATH, "%s/%s/exe", dir, file); + linksize = readlink(linkname, filename, MAX_PATH); + if (linksize <= 0) + return 0; + + filename[linksize] = 0; +#if 0 //check all process + if (strcmp(filename, myself)) + return 0; +#endif + + pid = atoi(file); + if (pid >= getpid()) + return 0; + + snprintf(linkname, MAX_PATH, "%s/%s/fd", dir, file); + if (!ls_dir(linkname, is_same_linkfile, argv)) + return 0; + + dbg_time("%s/%s/exe -> %s", dir, file, filename); + while (kill_timeout-- && !kill(pid, 0)) + { + kill(pid, SIGTERM); + sleep(1); + } + if (!kill(pid, 0)) + { + dbg_time("force kill %s/%s/exe -> %s", dir, file, filename); + kill(pid, SIGKILL); + sleep(1); + } + + return 1; +} + +static int kill_brothers(const char *qmichannel) +{ + char myself[MAX_PATH]; + int filenamesize; + void *argv[2] = {myself, (void *)qmichannel}; + + filenamesize = readlink("/proc/self/exe", myself, MAX_PATH); + if (filenamesize <= 0) + return 0; + myself[filenamesize] = 0; + + if (ls_dir("/proc", is_brother_process, argv)) + sleep(1); + + return 0; +} + +static void ql_sigaction(int signo) { + if (SIGCHLD == signo) + waitpid(-1, NULL, WNOHANG); + else if (SIGALRM == signo) + send_signo_to_main(SIGUSR1); + else + { + if (SIGTERM == signo || SIGHUP == signo || SIGINT == signo) + main_loop = 0; + send_signo_to_main(signo); + main_send_event_to_qmidevice(signo); //main may be wating qmi response + } +} + +pthread_t gQmiThreadID; + +static int usage(const char *progname) { + dbg_time("Usage: %s [-s [apn [user password auth]]] [-p pincode] [-f logfilename] ", progname); + dbg_time("-s [apn [user password auth]] Set apn/user/password/auth get from your network provider"); + dbg_time("-p pincode Verify sim card pin if sim card is locked"); + dbg_time("-f logfilename Save log message of this program to file"); + dbg_time("Example 1: %s ", progname); + dbg_time("Example 2: %s -s 3gnet ", progname); + dbg_time("Example 3: %s -s 3gnet carl 1234 0 -p 1234 -f gobinet_log.txt", progname); + return 0; +} +static int charsplit(const char *src,char* desc,int n,const char* splitStr) +{ + char* p; + char*p1; + int len; + + len=strlen(splitStr); + p=strstr(src,splitStr); + if(p==NULL) + return -1; + p1=strstr(p,"\n"); + if(p1==NULL) + return -1; + memset(desc,0,n); + memcpy(desc,p+len,p1-p-len); + + return 0; +} + +static int get_dev_major_minor(char* path, int *major, int *minor) +{ + int fd = -1; + char desc[128] = {0}; + char devmajor[64],devminor[64]; + int n = 0; + if(access(path, R_OK | W_OK)) + { + return 1; + } + if((fd = open(path, O_RDWR)) < 0) + { + return 1; + } + n = read(fd , desc, sizeof(desc)); + if(n == sizeof(desc)) + { + dbg_time("may be overflow"); + } + close(fd); + if(charsplit(desc,devmajor,64,"MAJOR=")==-1 || + charsplit(desc,devminor,64,"MINOR=")==-1 ) + { + return 2; + } + *major = atoi(devmajor); + *minor = atoi(devminor); + return 0; +} + +static int qmidevice_detect(char **pp_qmichannel, char **pp_usbnet_adapter) { + struct dirent* ent = NULL; + DIR *pDir; + + char dir[255] = "/sys/bus/usb/devices"; + int major = 0, minor = 0; + pDir = opendir(dir); + if (pDir) { + while ((ent = readdir(pDir)) != NULL) { + struct dirent* subent = NULL; + DIR *psubDir; + char subdir[255]; + char subdir2[255 * 2]; + + char idVendor[4+1] = {0}; + char idProduct[4+1] = {0}; + int fd = 0; + + char netcard[32] = "\0"; + char qmifile[32] = "\0"; + + snprintf(subdir, sizeof(subdir), "%s/%s/idVendor", dir, ent->d_name); + fd = open(subdir, O_RDONLY); + if (fd > 0) { + read(fd, idVendor, 4); + close(fd); + } + + snprintf(subdir, sizeof(subdir), "%s/%s/idProduct", dir, ent->d_name); + fd = open(subdir, O_RDONLY); + if (fd > 0) { + read(fd, idProduct, 4); + close(fd); + } + + if (!strncasecmp(idVendor, "05c6", 4) || !strncasecmp(idVendor, "2c7c", 4)) + ; + else + continue; + + dbg_time("Find %s/%s idVendor=%s idProduct=%s", dir, ent->d_name, idVendor, idProduct); + + snprintf(subdir, sizeof(subdir), "%s/%s:1.4/net", dir, ent->d_name); + psubDir = opendir(subdir); + if (psubDir == NULL) { + dbg_time("Cannot open directory: %s, errno: %d (%s)", subdir, errno, strerror(errno)); + continue; + } + + while ((subent = readdir(psubDir)) != NULL) { + if (subent->d_name[0] == '.') + continue; + dbg_time("Find %s/%s", subdir, subent->d_name); + dbg_time("Find usbnet_adapter = %s", subent->d_name); + strcpy(netcard, subent->d_name); + break; + } + + closedir(psubDir); + + if (netcard[0]) { + } else { + continue; + } + + if (*pp_usbnet_adapter && strcmp(*pp_usbnet_adapter, netcard)) + continue; + + snprintf(subdir, sizeof(subdir), "%s/%s:1.4/GobiQMI", dir, ent->d_name); + if (access(subdir, R_OK)) { + snprintf(subdir, sizeof(subdir), "%s/%s:1.4/usbmisc", dir, ent->d_name); + if (access(subdir, R_OK)) { + snprintf(subdir, sizeof(subdir), "%s/%s:1.4/usb", dir, ent->d_name); + if (access(subdir, R_OK)) { + dbg_time("no GobiQMI/usbmic/usb found in %s/%s:1.4", dir, ent->d_name); + continue; + } + } + } + + psubDir = opendir(subdir); + if (pDir == NULL) { + dbg_time("Cannot open directory: %s, errno: %d (%s)", dir, errno, strerror(errno)); + continue; + } + + while ((subent = readdir(psubDir)) != NULL) { + if (subent->d_name[0] == '.') + continue; + dbg_time("Find %s/%s", subdir, subent->d_name); + dbg_time("Find qmichannel = /dev/%s", subent->d_name); + snprintf(qmifile, sizeof(qmifile), "/dev/%s", subent->d_name); + + //get major minor + snprintf(subdir2, sizeof(subdir), "%s/%s/uevent",subdir, subent->d_name); + if(!get_dev_major_minor(subdir2, &major, &minor)) + { + //dbg_time("%s major = %d, minor = %d\n",qmifile, major, minor); + }else + { + dbg_time("get %s major and minor failed\n",qmifile); + } + //get major minor + + if((fd = open(qmifile, R_OK)) < 0) + { + dbg_time("%s open failed", qmifile); + dbg_time("please mknod %s c %d %d", qmifile, major, minor); + }else + { + close(fd); + } + break; + } + + closedir(psubDir); + + if (netcard[0] && qmifile[0]) { + *pp_qmichannel = strdup(qmifile); + *pp_usbnet_adapter = strdup(netcard); + closedir(pDir); + return 1; + } + + } + + closedir(pDir); + } + + if ((pDir = opendir("/dev")) == NULL) { + dbg_time("Cannot open directory: %s, errno:%d (%s)", "/dev", errno, strerror(errno)); + return -ENODEV; + } + + while ((ent = readdir(pDir)) != NULL) { + if ((strncmp(ent->d_name, "cdc-wdm", strlen("cdc-wdm")) == 0) || (strncmp(ent->d_name, "qcqmi", strlen("qcqmi")) == 0)) { + char net_path[64]; + + *pp_qmichannel = (char *)malloc(32); + sprintf(*pp_qmichannel, "/dev/%s", ent->d_name); + dbg_time("Find qmichannel = %s", *pp_qmichannel); + + if (strncmp(ent->d_name, "cdc-wdm", strlen("cdc-wdm")) == 0) + sprintf(net_path, "/sys/class/net/wwan%s", &ent->d_name[strlen("cdc-wdm")]); + else + { + sprintf(net_path, "/sys/class/net/usb%s", &ent->d_name[strlen("qcqmi")]); + #if 0//ndef ANDROID + if (kernel_version >= KVERSION( 2,6,39 )) + sprintf(net_path, "/sys/class/net/eth%s", &ent->d_name[strlen("qcqmi")]); + #else + if (access(net_path, R_OK) && errno == ENOENT) + sprintf(net_path, "/sys/class/net/eth%s", &ent->d_name[strlen("qcqmi")]); + #endif +#if 0 //openWRT like use ppp# or lte# + if (access(net_path, R_OK) && errno == ENOENT) + sprintf(net_path, "/sys/class/net/ppp%s", &ent->d_name[strlen("qcqmi")]); + if (access(net_path, R_OK) && errno == ENOENT) + sprintf(net_path, "/sys/class/net/lte%s", &ent->d_name[strlen("qcqmi")]); +#endif + } + + if (access(net_path, R_OK) == 0) + { + if (*pp_usbnet_adapter && strcmp(*pp_usbnet_adapter, (net_path + strlen("/sys/class/net/")))) + { + free(*pp_qmichannel); *pp_qmichannel = NULL; + continue; + } + *pp_usbnet_adapter = strdup(net_path + strlen("/sys/class/net/")); + dbg_time("Find usbnet_adapter = %s", *pp_usbnet_adapter); + break; + } + else + { + dbg_time("Failed to access %s, errno:%d (%s)", net_path, errno, strerror(errno)); + free(*pp_qmichannel); *pp_qmichannel = NULL; + } + } + } + closedir(pDir); + + return (*pp_qmichannel && *pp_usbnet_adapter); +} + +#if defined(ANDROID) || defined(LINUX_RIL_SHLIB) +int quectel_CM(int argc, char *argv[]) +#else +int main(int argc, char *argv[]) +#endif +{ + int triger_event = 0; + int opt = 1; + int signo; +#ifdef CONFIG_SIM + SIM_Status SIMStatus; +#endif + UCHAR PSAttachedState; + UCHAR IPv4ConnectionStatus = 0xff; //unknow state + UCHAR IPV6ConnectionStatus = 0xff; //unknow state + int qmierr = 0; + char * save_usbnet_adapter = NULL; + PROFILE_T profile; +#ifdef CONFIG_RESET_RADIO + struct timeval resetRadioTime = {0}; + struct timeval nowTime; + gettimeofday(&resetRadioTime, (struct timezone *) NULL); +#endif + + memset(&profile, 0x00, sizeof(profile)); + profile.pdp = CONFIG_DEFAULT_PDP; + + if (!strcmp(argv[argc-1], "&")) + argc--; + + opt = 1; + while (opt < argc) + { + if (argv[opt][0] != '-') + return usage(argv[0]); + + switch (argv[opt++][1]) + { +#define has_more_argv() ((opt < argc) && (argv[opt][0] != '-')) + case 's': + profile.apn = profile.user = profile.password = ""; + if (has_more_argv()) + profile.apn = argv[opt++]; + if (has_more_argv()) + profile.user = argv[opt++]; + if (has_more_argv()) + { + profile.password = argv[opt++]; + if (profile.password && profile.password[0]) + profile.auth = 2; //default chap, customers may miss auth + } + if (has_more_argv()) + profile.auth = argv[opt++][0] - '0'; + break; + + case 'p': + if (has_more_argv()) + profile.pincode = argv[opt++]; + break; + + case 'n': + if (has_more_argv()) + profile.pdp = argv[opt++][0] - '0'; + break; + + case 'f': + if (has_more_argv()) + { + const char * filename = argv[opt++]; + logfilefp = fopen(filename, "a+"); + if (!logfilefp) { + dbg_time("Fail to open %s, errno: %d(%s)", filename, errno, strerror(errno)); + } + } + break; + + case 'i': + if (has_more_argv()) + profile.usbnet_adapter = save_usbnet_adapter = argv[opt++]; + break; + + case 'v': + debug_qmi = 1; + break; + + case 'l': + main_loop = 1; + break; + + case '6': + profile.IsDualIPSupported |= (1 << IpFamilyV6); //support ipv4&ipv6 + break; + + case 'd': + if (has_more_argv()) { + profile.qmichannel = argv[opt++]; + if (!strncmp(profile.qmichannel, "/dev/mhi_QMI", strlen("/dev/mhi_QMI"))) + profile.usbnet_adapter = "rmnet_mhi0"; + } + break; + + default: + return usage(argv[0]); + break; + } + } + + dbg_time("WCDMA<E_QConnectManager_Linux&Android_V1.1.45"); + dbg_time("%s profile[%d] = %s/%s/%s/%d, pincode = %s", argv[0], profile.pdp, profile.apn, profile.user, profile.password, profile.auth, profile.pincode); + + signal(SIGUSR1, ql_sigaction); + signal(SIGUSR2, ql_sigaction); + signal(SIGINT, ql_sigaction); + signal(SIGTERM, ql_sigaction); + signal(SIGHUP, ql_sigaction); + signal(SIGCHLD, ql_sigaction); + signal(SIGALRM, ql_sigaction); + +#ifdef CONFIG_BACKGROUND_WHEN_GET_IP + ql_prepare_daemon(); +#endif + + if (socketpair( AF_LOCAL, SOCK_STREAM, 0, signal_control_fd) < 0 ) { + dbg_time("%s Faild to create main_control_fd: %d (%s)", __func__, errno, strerror(errno)); + return -1; + } + + if ( socketpair( AF_LOCAL, SOCK_STREAM, 0, qmidevice_control_fd ) < 0 ) { + dbg_time("%s Failed to create thread control socket pair: %d (%s)", __func__, errno, strerror(errno)); + return 0; + } + +//sudo apt-get install udhcpc +//sudo apt-get remove ModemManager +__main_loop: + while (!profile.qmichannel) + { + if (qmidevice_detect(&profile.qmichannel, &profile.usbnet_adapter)) + break; + if (main_loop) + { + int wait_for_device = 3000; + dbg_time("Wait for Quectel modules connect"); + while (wait_for_device && main_loop) { + wait_for_device -= 100; + usleep(100*1000); + } + continue; + } + dbg_time("Cannot find qmichannel(%s) usbnet_adapter(%s) for Quectel modules", profile.qmichannel, profile.usbnet_adapter); + return -ENODEV; + } + + if (!strncmp(profile.qmichannel, "/dev/qcqmi", strlen("/dev/qcqmi"))) { + char MODE_FILE[128]; + int mode_fd; + + snprintf(MODE_FILE, sizeof(MODE_FILE), "/sys/class/net/%s/qmap_mode", profile.usbnet_adapter); + mode_fd = open(MODE_FILE, O_RDONLY); + + if (mode_fd > 0) { + char qmap_mode[2] = {0, 0}; + read(mode_fd, &qmap_mode, sizeof(qmap_mode)); + close(mode_fd); + profile.qmap_mode = qmap_mode[0] - '0'; + } + + if (profile.qmap_mode) { + char qmapnet_adapter[128]; + + sprintf(qmapnet_adapter, "/sys/class/net/%s.%d", profile.usbnet_adapter, profile.pdp); + if (!access(qmapnet_adapter, R_OK)) { + static char netcard[32] = "\0"; + + sprintf(netcard, "%s.%d", profile.usbnet_adapter, profile.pdp); + profile.qmapnet_adapter = netcard; + } else { + profile.qmapnet_adapter = profile.usbnet_adapter; + } + dbg_time("Find qmapnet_adapter = %s", profile.qmapnet_adapter); + } + } else if(!strncmp(profile.qmichannel, "/dev/cdc-wdm", strlen("/dev/cdc-wdm"))) { + if (!access("/sys/module/qmi_wwan/parameters/rx_qmap", R_OK)) { + char qmapnet_adapter[128]; + + sprintf(qmapnet_adapter, "/sys/class/net/%s.%d", profile.usbnet_adapter, profile.pdp); + if (!access(qmapnet_adapter, R_OK)) { + static char netcard[32] = "\0"; + + sprintf(netcard, "%s.%d", profile.usbnet_adapter, profile.pdp); + profile.qmapnet_adapter = netcard; + } else { + profile.qmapnet_adapter = profile.usbnet_adapter; + } + dbg_time("Find qmapnet_adapter = %s", profile.qmapnet_adapter); + } + } else if (!strncmp(profile.qmichannel, "/dev/mhi_QMI", strlen("/dev/mhi_QMI"))) { + profile.qmapnet_adapter = profile.usbnet_adapter; + } + + if (access(profile.qmichannel, R_OK | W_OK)) { + dbg_time("Fail to access %s, errno: %d (%s)", profile.qmichannel, errno, strerror(errno)); + return errno; + } + +#if 0 //for test only, make fd > 255 +{ + int max_dup = 255; + while (max_dup--) + dup(0); +} +#endif + + if (profile.qmapnet_adapter == NULL || profile.qmapnet_adapter == profile.usbnet_adapter) + kill_brothers(profile.qmichannel); + + qmichannel = profile.qmichannel; + if (!strncmp(profile.qmichannel, "/dev/qcqmi", strlen("/dev/qcqmi"))) + { + if (pthread_create( &gQmiThreadID, 0, GobiNetThread, (void *)&profile) != 0) + { + dbg_time("%s Failed to create GobiNetThread: %d (%s)", __func__, errno, strerror(errno)); + return 0; + } + } + else + { + if (pthread_create( &gQmiThreadID, 0, QmiWwanThread, (void *)&profile) != 0) + { + dbg_time("%s Failed to create QmiWwanThread: %d (%s)", __func__, errno, strerror(errno)); + return 0; + } + } + + if ((read(qmidevice_control_fd[0], &triger_event, sizeof(triger_event)) != sizeof(triger_event)) + || (triger_event != RIL_INDICATE_DEVICE_CONNECTED)) { + dbg_time("%s Failed to init QMIThread: %d (%s)", __func__, errno, strerror(errno)); + return 0; + } + + if (!strncmp(profile.qmichannel, "/dev/cdc-wdm", strlen("/dev/cdc-wdm")) + || !strncmp(profile.qmichannel, "/dev/mhi_QMI", strlen("/dev/mhi_QMI"))) { + if (QmiWwanInit(&profile)) { + dbg_time("%s Failed to QmiWwanInit: %d (%s)", __func__, errno, strerror(errno)); + return 0; + } + } + +#ifdef CONFIG_VERSION + requestBaseBandVersion(NULL); +#endif + requestSetEthMode(&profile); +#if 0 + if (profile.rawIP && !strncmp(profile.qmichannel, "/dev/cdc-wdm", strlen("/dev/cdc-wdm"))) { + char raw_ip_switch[128] = {0}; + sprintf(raw_ip_switch, "/sys/class/net/%s/qmi/raw_ip", profile.usbnet_adapter); + if (!access(raw_ip_switch, R_OK)) { + int raw_ip_fd = -1; + raw_ip_fd = open(raw_ip_switch, O_RDWR); + if (raw_ip_fd >= 0) { + write(raw_ip_fd, "1", strlen("1")); + close(raw_ip_fd); + raw_ip_fd = -1; + } else { + dbg_time("open %s failed, errno = %d(%s)\n", raw_ip_switch, errno, strerror(errno)); + } + } + } +#endif +#ifdef CONFIG_SIM + qmierr = requestGetSIMStatus(&SIMStatus); + while (qmierr == QMI_ERR_OP_DEVICE_UNSUPPORTED) { + sleep(1); + qmierr = requestGetSIMStatus(&SIMStatus); + } + if ((SIMStatus == SIM_PIN) && profile.pincode) { + requestEnterSimPin(profile.pincode); + } +#ifdef CONFIG_IMSI_ICCID + if (SIMStatus == SIM_READY) { + requestGetICCID(); + requestGetIMSI(); + } +#endif +#endif +#ifdef CONFIG_APN + if (profile.apn || profile.user || profile.password) { + requestSetProfile(&profile); + } + requestGetProfile(&profile); +#endif + requestRegistrationState(&PSAttachedState); + + if (!requestQueryDataCall(&IPv4ConnectionStatus, IpFamilyV4) && (QWDS_PKT_DATA_CONNECTED == IPv4ConnectionStatus)) + usbnet_link_change(1, &profile); + else + usbnet_link_change(0, &profile); + + send_signo_to_main(SIGUSR1); + +#ifdef CONFIG_PID_FILE_FORMAT + { + char cmd[255]; + sprintf(cmd, "echo %d > " CONFIG_PID_FILE_FORMAT, getpid(), profile.usbnet_adapter); + system(cmd); + } +#endif + + while (1) + { + struct pollfd pollfds[] = {{signal_control_fd[1], POLLIN, 0}, {qmidevice_control_fd[0], POLLIN, 0}}; + int ne, ret, nevents = sizeof(pollfds)/sizeof(pollfds[0]); + + do { + ret = poll(pollfds, nevents, 15*1000); + } while ((ret < 0) && (errno == EINTR)); + + if (ret == 0) + { + send_signo_to_main(SIGUSR2); + continue; + } + + if (ret <= 0) { + dbg_time("%s poll=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + goto __main_quit; + } + + for (ne = 0; ne < nevents; ne++) { + int fd = pollfds[ne].fd; + short revents = pollfds[ne].revents; + + if (revents & (POLLERR | POLLHUP | POLLNVAL)) { + dbg_time("%s poll err/hup", __func__); + dbg_time("epoll fd = %d, events = 0x%04x", fd, revents); + main_send_event_to_qmidevice(RIL_REQUEST_QUIT); + if (revents & POLLHUP) + goto __main_quit; + } + + if ((revents & POLLIN) == 0) + continue; + + if (fd == signal_control_fd[1]) + { + if (read(fd, &signo, sizeof(signo)) == sizeof(signo)) + { + alarm(0); + switch (signo) + { + case SIGUSR1: + requestQueryDataCall(&IPv4ConnectionStatus, IpFamilyV4); + if (QWDS_PKT_DATA_CONNECTED != IPv4ConnectionStatus) + { + usbnet_link_change(0, &profile); + requestRegistrationState(&PSAttachedState); + + if (PSAttachedState == 1) { + qmierr = requestSetupDataCall(&profile, IpFamilyV4); + + if ((qmierr > 0) && profile.user && profile.user[0] && profile.password && profile.password[0]) { + int old_auto = profile.auth; + + //may be fail because wrong auth mode, try pap->chap, or chap->pap + profile.auth = (profile.auth == 1) ? 2 : 1; + qmierr = requestSetupDataCall(&profile, IpFamilyV4); + + if (qmierr) + profile.auth = old_auto; //still fail, restore old auth moe + } + + //succssful setup data call + if (!qmierr && profile.IsDualIPSupported) { + requestSetupDataCall(&profile, IpFamilyV6); + } + + if (!qmierr) + continue; + } + +#ifdef CONFIG_EXIT_WHEN_DIAL_FAILED + kill(getpid(), SIGTERM); +#endif +#ifdef CONFIG_RESET_RADIO + gettimeofday(&nowTime, (struct timezone *) NULL); + if (abs(nowTime.tv_sec - resetRadioTime.tv_sec) > CONFIG_RESET_RADIO) { + resetRadioTime = nowTime; + //requestSetOperatingMode(0x06); //same as AT+CFUN=0 + requestSetOperatingMode(0x01); //same as AT+CFUN=4 + requestSetOperatingMode(0x00); //same as AT+CFUN=1 + } +#endif + alarm(5); //try to setup data call 5 seconds later + } + break; + + case SIGUSR2: + if (QWDS_PKT_DATA_CONNECTED == IPv4ConnectionStatus) + requestQueryDataCall(&IPv4ConnectionStatus, IpFamilyV4); + + //local ip is different with remote ip + if (QWDS_PKT_DATA_CONNECTED == IPv4ConnectionStatus && check_ipv4_address(&profile) == 0) { + requestDeactivateDefaultPDP(&profile, IpFamilyV4); + IPv4ConnectionStatus = QWDS_PKT_DATA_DISCONNECTED; + } + + if (QWDS_PKT_DATA_CONNECTED != IPv4ConnectionStatus) { + #if defined(ANDROID) || defined(LINUX_RIL_SHLIB) + kill(getpid(), SIGTERM); //android will setup data call again + #else + send_signo_to_main(SIGUSR1); + #endif + } + break; + + case SIGTERM: + case SIGHUP: + case SIGINT: + if (QWDS_PKT_DATA_CONNECTED == IPv4ConnectionStatus) { + requestDeactivateDefaultPDP(&profile, IpFamilyV4); + if (profile.IsDualIPSupported) + requestDeactivateDefaultPDP(&profile, IpFamilyV6); + } + usbnet_link_change(0, &profile); + if (!strncmp(profile.qmichannel, "/dev/cdc-wdm", strlen("/dev/cdc-wdm")) + || !strncmp(profile.qmichannel, "/dev/mhi_QMI", strlen("/dev/mhi_QMI"))) + QmiWwanDeInit(); + main_send_event_to_qmidevice(RIL_REQUEST_QUIT); + goto __main_quit; + break; + + default: + break; + } + } + } + + if (fd == qmidevice_control_fd[0]) { + if (read(fd, &triger_event, sizeof(triger_event)) == sizeof(triger_event)) { + switch (triger_event) { + case RIL_INDICATE_DEVICE_DISCONNECTED: + usbnet_link_change(0, &profile); + if (main_loop) + { + if (pthread_join(gQmiThreadID, NULL)) { + dbg_time("%s Error joining to listener thread (%s)", __func__, strerror(errno)); + } + profile.qmichannel = NULL; + profile.usbnet_adapter = save_usbnet_adapter; + goto __main_loop; + } + goto __main_quit; + break; + + case RIL_UNSOL_RESPONSE_VOICE_NETWORK_STATE_CHANGED: + requestRegistrationState(&PSAttachedState); + if (PSAttachedState == 1 && QWDS_PKT_DATA_DISCONNECTED == IPv4ConnectionStatus) + send_signo_to_main(SIGUSR1); + break; + + case RIL_UNSOL_DATA_CALL_LIST_CHANGED: + { + UCHAR oldConnectionStatus = IPv4ConnectionStatus; + requestQueryDataCall(&IPv4ConnectionStatus, IpFamilyV4); + if (profile.IsDualIPSupported) + requestQueryDataCall(&IPV6ConnectionStatus, IpFamilyV6); + if (QWDS_PKT_DATA_CONNECTED != IPv4ConnectionStatus) + { + usbnet_link_change(0, &profile); + if (oldConnectionStatus == QWDS_PKT_DATA_CONNECTED){ //connected change to disconnect +#if defined(ANDROID) || defined(LINUX_RIL_SHLIB) + kill(getpid(), SIGTERM); //android will setup data call again +#else + send_signo_to_main(SIGUSR1); +#endif + } + } else if (QWDS_PKT_DATA_CONNECTED == IPv4ConnectionStatus) { + usbnet_link_change(1, &profile); + if (oldConnectionStatus == QWDS_PKT_DATA_CONNECTED) { //receive two CONNECT IND? + send_signo_to_main(SIGUSR2); + } + } + } + break; + + default: + break; + } + } + } + } + } + +__main_quit: + usbnet_link_change(0, &profile); + if (pthread_join(gQmiThreadID, NULL)) { + dbg_time("%s Error joining to listener thread (%s)", __func__, strerror(errno)); + } + close(signal_control_fd[0]); + close(signal_control_fd[1]); + close(qmidevice_control_fd[0]); + close(qmidevice_control_fd[1]); + dbg_time("%s exit", __func__); + if (logfilefp) + fclose(logfilefp); + +#ifdef CONFIG_PID_FILE_FORMAT + { + char cmd[255]; + sprintf(cmd, "rm " CONFIG_PID_FILE_FORMAT, profile.usbnet_adapter); + system(cmd); + } +#endif + + return 0; +} diff --git a/quectel-CM/quectel-qmi-proxy.c b/quectel-CM/quectel-qmi-proxy.c new file mode 100755 index 0000000..d7c43f3 --- /dev/null +++ b/quectel-CM/quectel-qmi-proxy.c @@ -0,0 +1,1131 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned int uint32_t; + +#define cpu_to_le16(x) (x) +#define cpu_to_le32(x) (x) +#define le16_to_cpu(x) (x) +#define le32_to_cpu(x) (x) + +#define dprintf(fmt, arg...) do { printf(fmt, ##arg); } while(0) +#define SYSCHECK(c) do{if((c)<0) {dprintf("%s %d error: '%s' (code: %d)\n", __func__, __LINE__, strerror(errno), errno); return -1;}}while(0) +#define cfmakenoblock(fd) do{fcntl(fd, F_SETFL, fcntl(fd,F_GETFL) | O_NONBLOCK);}while(0) + +typedef struct _QCQMI_HDR +{ + uint8_t IFType; + uint16_t Length; + uint8_t CtlFlags; // reserved + uint8_t QMIType; + uint8_t ClientId; +} __attribute__ ((packed)) QCQMI_HDR, *PQCQMI_HDR; + +typedef struct _QMICTL_SYNC_REQ_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_REQUEST + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_CTL_SYNC_REQ + uint16_t Length; // 0 +} __attribute__ ((packed)) QMICTL_SYNC_REQ_MSG, *PQMICTL_SYNC_REQ_MSG; + +typedef struct _QMICTL_SYNC_RESP_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_CTL_SYNC_RESP + uint16_t Length; + uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE + uint16_t TLVLength; // 0x0004 + uint16_t QMIResult; + uint16_t QMIError; +} __attribute__ ((packed)) QMICTL_SYNC_RESP_MSG, *PQMICTL_SYNC_RESP_MSG; + +typedef struct _QMICTL_SYNC_IND_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_INDICATION + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_REVOKE_CLIENT_ID_IND + uint16_t Length; +} __attribute__ ((packed)) QMICTL_SYNC_IND_MSG, *PQMICTL_SYNC_IND_MSG; + +typedef struct _QMICTL_GET_CLIENT_ID_REQ_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_REQUEST + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_GET_CLIENT_ID_REQ + uint16_t Length; + uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + uint16_t TLVLength; // 1 + uint8_t QMIType; // QMUX type +} __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_REQ_MSG, *PQMICTL_GET_CLIENT_ID_REQ_MSG; + +typedef struct _QMICTL_GET_CLIENT_ID_RESP_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_GET_CLIENT_ID_RESP + uint16_t Length; + uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE + uint16_t TLVLength; // 0x0004 + uint16_t QMIResult; // result code + uint16_t QMIError; // error code + uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + uint16_t TLV2Length; // 2 + uint8_t QMIType; + uint8_t ClientId; +} __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_RESP_MSG, *PQMICTL_GET_CLIENT_ID_RESP_MSG; + +typedef struct _QMICTL_RELEASE_CLIENT_ID_REQ_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_REQUEST + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_RELEASE_CLIENT_ID_REQ + uint16_t Length; + uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + uint16_t TLVLength; // 0x0002 + uint8_t QMIType; + uint8_t ClientId; +} __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_REQ_MSG, *PQMICTL_RELEASE_CLIENT_ID_REQ_MSG; + +typedef struct _QMICTL_RELEASE_CLIENT_ID_RESP_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_RELEASE_CLIENT_ID_RESP + uint16_t Length; + uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE + uint16_t TLVLength; // 0x0004 + uint16_t QMIResult; // result code + uint16_t QMIError; // error code + uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + uint16_t TLV2Length; // 2 + uint8_t QMIType; + uint8_t ClientId; +} __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_RESP_MSG, *PQMICTL_RELEASE_CLIENT_ID_RESP_MSG; + +// QMICTL Control Flags +#define QMICTL_CTL_FLAG_CMD 0x00 +#define QMICTL_CTL_FLAG_RSP 0x01 +#define QMICTL_CTL_FLAG_IND 0x02 + +typedef struct _QCQMICTL_MSG_HDR +{ + uint8_t CtlFlags; // 00-cmd, 01-rsp, 10-ind + uint8_t TransactionId; + uint16_t QMICTLType; + uint16_t Length; +} __attribute__ ((packed)) QCQMICTL_MSG_HDR, *PQCQMICTL_MSG_HDR; + +#define QCQMICTL_MSG_HDR_SIZE sizeof(QCQMICTL_MSG_HDR) + +typedef struct _QCQMICTL_MSG_HDR_RESP +{ + uint8_t CtlFlags; // 00-cmd, 01-rsp, 10-ind + uint8_t TransactionId; + uint16_t QMICTLType; + uint16_t Length; + uint8_t TLVType; // 0x02 - result code + uint16_t TLVLength; // 4 + uint16_t QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + uint16_t QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} __attribute__ ((packed)) QCQMICTL_MSG_HDR_RESP, *PQCQMICTL_MSG_HDR_RESP; + +typedef struct _QMICTL_GET_VERSION_REQ_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_REQUEST + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_GET_VERSION_REQ + uint16_t Length; // 0 + uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER + uint16_t TLVLength; // var + uint8_t QMUXTypes; // List of one byte QMUX_TYPE values + // 0xFF returns a list of versions for all + // QMUX_TYPEs implemented on the device +} __attribute__ ((packed)) QMICTL_GET_VERSION_REQ_MSG, *PQMICTL_GET_VERSION_REQ_MSG; + +typedef struct _QMUX_TYPE_VERSION_STRUCT +{ + uint8_t QMUXType; + uint16_t MajorVersion; + uint16_t MinorVersion; +} __attribute__ ((packed)) QMUX_TYPE_VERSION_STRUCT, *PQMUX_TYPE_VERSION_STRUCT; + +typedef struct _QMICTL_GET_VERSION_RESP_MSG +{ + uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE + uint8_t TransactionId; + uint16_t QMICTLType; // QMICTL_GET_VERSION_RESP + uint16_t Length; + uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE + uint16_t TLVLength; // 0x0004 + uint16_t QMIResult; + uint16_t QMIError; + uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER + uint16_t TLV2Length; // var + uint8_t NumElements; // Num of QMUX_TYPE_VERSION_STRUCT + QMUX_TYPE_VERSION_STRUCT TypeVersion[0]; +} __attribute__ ((packed)) QMICTL_GET_VERSION_RESP_MSG, *PQMICTL_GET_VERSION_RESP_MSG; + + +typedef struct _QMICTL_MSG +{ + union + { + // Message Header + QCQMICTL_MSG_HDR QMICTLMsgHdr; + QCQMICTL_MSG_HDR_RESP QMICTLMsgHdrRsp; + + // QMICTL Message + //QMICTL_SET_INSTANCE_ID_REQ_MSG SetInstanceIdReq; + //QMICTL_SET_INSTANCE_ID_RESP_MSG SetInstanceIdRsp; + QMICTL_GET_VERSION_REQ_MSG GetVersionReq; + QMICTL_GET_VERSION_RESP_MSG GetVersionRsp; + QMICTL_GET_CLIENT_ID_REQ_MSG GetClientIdReq; + QMICTL_GET_CLIENT_ID_RESP_MSG GetClientIdRsp; + //QMICTL_RELEASE_CLIENT_ID_REQ_MSG ReleaseClientIdReq; + QMICTL_RELEASE_CLIENT_ID_RESP_MSG ReleaseClientIdRsp; + //QMICTL_REVOKE_CLIENT_ID_IND_MSG RevokeClientIdInd; + //QMICTL_INVALID_CLIENT_ID_IND_MSG InvalidClientIdInd; + //QMICTL_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq; + //QMICTL_SET_DATA_FORMAT_RESP_MSG SetDataFormatRsp; + QMICTL_SYNC_REQ_MSG SyncReq; + QMICTL_SYNC_RESP_MSG SyncRsp; + QMICTL_SYNC_IND_MSG SyncInd; + }; +} __attribute__ ((packed)) QMICTL_MSG, *PQMICTL_MSG; + +typedef struct _QCQMUX_MSG_HDR +{ + uint8_t CtlFlags; // 0: single QMUX Msg; 1: + uint16_t TransactionId; + uint16_t Type; + uint16_t Length; + uint8_t payload[0]; +} __attribute__ ((packed)) QCQMUX_MSG_HDR, *PQCQMUX_MSG_HDR; + +typedef struct _QCQMUX_MSG_HDR_RESP +{ + uint8_t CtlFlags; // 0: single QMUX Msg; 1: + uint16_t TransactionId; + uint16_t Type; + uint16_t Length; + uint8_t TLVType; // 0x02 - result code + uint16_t TLVLength; // 4 + uint16_t QMUXResult; // QMI_RESULT_SUCCESS + // QMI_RESULT_FAILURE + uint16_t QMUXError; // QMI_ERR_INVALID_ARG + // QMI_ERR_NO_MEMORY + // QMI_ERR_INTERNAL + // QMI_ERR_FAULT +} __attribute__ ((packed)) QCQMUX_MSG_HDR_RESP, *PQCQMUX_MSG_HDR_RESP; + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT +{ + uint16_t Type; // QMUX type 0x0000 + uint16_t Length; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT, *PQMIWDS_ADMIN_SET_DATA_FORMAT; + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS +{ + uint8_t TLVType; + uint16_t TLVLength; + uint8_t QOSSetting; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS, *PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS; + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_TLV +{ + uint8_t TLVType; + uint16_t TLVLength; + uint32_t Value; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_TLV, *PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV; + +typedef struct _QMIWDS_ENDPOINT_TLV +{ + uint8_t TLVType; + uint16_t TLVLength; + uint32_t ep_type; + uint32_t iface_id; +} __attribute__ ((packed)) QMIWDS_ENDPOINT_TLV, *PQMIWDS_ENDPOINT_TLV; + +typedef struct _QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG +{ + uint8_t CtlFlags; // 0: single QMUX Msg; 1: + uint16_t TransactionId; + uint16_t Type; + uint16_t Length; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV_QOS QosDataFormatTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UnderlyingLinkLayerProtocolTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV UplinkDataAggregationProtocolTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationProtocolTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationMaxDatagramsTlv; + QMIWDS_ADMIN_SET_DATA_FORMAT_TLV DownlinkDataAggregationMaxSizeTlv; + QMIWDS_ENDPOINT_TLV epTlv; +} __attribute__ ((packed)) QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG, *PQMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG; + +typedef struct _QCQMUX_TLV +{ + uint8_t Type; + uint16_t Length; + uint8_t Value[0]; +} __attribute__ ((packed)) QCQMUX_TLV, *PQCQMUX_TLV; + +typedef struct _QMUX_MSG +{ + union + { + // Message Header + QCQMUX_MSG_HDR QMUXMsgHdr; + QCQMUX_MSG_HDR_RESP QMUXMsgHdrResp; + QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq; + }; +} __attribute__ ((packed)) QMUX_MSG, *PQMUX_MSG; + +typedef struct _QCQMIMSG { + QCQMI_HDR QMIHdr; + union { + QMICTL_MSG CTLMsg; + QMUX_MSG MUXMsg; + }; +} __attribute__ ((packed)) QCQMIMSG, *PQCQMIMSG; + + +// QMUX Message Definitions -- QMI SDU +#define QMUX_CTL_FLAG_SINGLE_MSG 0x00 +#define QMUX_CTL_FLAG_COMPOUND_MSG 0x01 +#define QMUX_CTL_FLAG_TYPE_CMD 0x00 +#define QMUX_CTL_FLAG_TYPE_RSP 0x02 +#define QMUX_CTL_FLAG_TYPE_IND 0x04 +#define QMUX_CTL_FLAG_MASK_COMPOUND 0x01 +#define QMUX_CTL_FLAG_MASK_TYPE 0x06 // 00-cmd, 01-rsp, 10-ind + +#define USB_CTL_MSG_TYPE_QMI 0x01 + +#define QMICTL_FLAG_REQUEST 0x00 +#define QMICTL_FLAG_RESPONSE 0x01 +#define QMICTL_FLAG_INDICATION 0x02 + +// QMICTL Type +#define QMICTL_SET_INSTANCE_ID_REQ 0x0020 +#define QMICTL_SET_INSTANCE_ID_RESP 0x0020 +#define QMICTL_GET_VERSION_REQ 0x0021 +#define QMICTL_GET_VERSION_RESP 0x0021 +#define QMICTL_GET_CLIENT_ID_REQ 0x0022 +#define QMICTL_GET_CLIENT_ID_RESP 0x0022 +#define QMICTL_RELEASE_CLIENT_ID_REQ 0x0023 +#define QMICTL_RELEASE_CLIENT_ID_RESP 0x0023 +#define QMICTL_REVOKE_CLIENT_ID_IND 0x0024 +#define QMICTL_INVALID_CLIENT_ID_IND 0x0025 +#define QMICTL_SET_DATA_FORMAT_REQ 0x0026 +#define QMICTL_SET_DATA_FORMAT_RESP 0x0026 +#define QMICTL_SYNC_REQ 0x0027 +#define QMICTL_SYNC_RESP 0x0027 +#define QMICTL_SYNC_IND 0x0027 + +#define QCTLV_TYPE_REQUIRED_PARAMETER 0x01 + +// Define QMI Type +typedef enum _QMI_SERVICE_TYPE +{ + QMUX_TYPE_CTL = 0x00, + QMUX_TYPE_WDS = 0x01, + QMUX_TYPE_DMS = 0x02, + QMUX_TYPE_NAS = 0x03, + QMUX_TYPE_QOS = 0x04, + QMUX_TYPE_WMS = 0x05, + QMUX_TYPE_PDS = 0x06, + QMUX_TYPE_UIM = 0x0B, + QMUX_TYPE_WDS_IPV6 = 0x11, + QMUX_TYPE_WDS_ADMIN = 0x1A, + QMUX_TYPE_MAX = 0xFF, + QMUX_TYPE_ALL = 0xFF +} QMI_SERVICE_TYPE; + +#define QMIWDS_ADMIN_SET_DATA_FORMAT_REQ 0x0020 +#define QMIWDS_ADMIN_SET_DATA_FORMAT_RESP 0x0020 + +struct qlistnode +{ + struct qlistnode *next; + struct qlistnode *prev; +}; + +#define qnode_to_item(node, container, member) \ + (container *) (((char*) (node)) - offsetof(container, member)) + +#define qlist_for_each(node, list) \ + for (node = (list)->next; node != (list); node = node->next) + +#define qlist_empty(list) ((list) == (list)->next) +#define qlist_head(list) ((list)->next) +#define qlist_tail(list) ((list)->prev) + +typedef struct { + struct qlistnode qnode; + uint8_t ClientFd; + QCQMIMSG qmi[0]; +} QMI_PROXY_MSG; + +typedef struct { + struct qlistnode qnode; + uint8_t QMIType; + uint8_t ClientId; + unsigned AccessTime; +} QMI_PROXY_CLINET; + +typedef struct { + struct qlistnode qnode; + struct qlistnode client_qnode; + uint8_t ClientFd; + unsigned AccessTime; +} QMI_PROXY_CONNECTION; + +static void qlist_init(struct qlistnode *node) +{ + node->next = node; + node->prev = node; +} + +static void qlist_add_tail(struct qlistnode *head, struct qlistnode *item) +{ + item->next = head; + item->prev = head->prev; + head->prev->next = item; + head->prev = item; +} + +static void qlist_remove(struct qlistnode *item) +{ + item->next->prev = item->prev; + item->prev->next = item->next; +} + +static int cdc_wdm_fd = -1; +static int qmi_proxy_server_fd = -1; +static struct qlistnode qmi_proxy_connection; +static struct qlistnode qmi_proxy_ctl_msg; +static PQCQMIMSG s_pCtlReq; +static PQCQMIMSG s_pCtlRsq; +static pthread_mutex_t s_ctlmutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t s_ctlcond = PTHREAD_COND_INITIALIZER; + +static void setTimespecRelative(struct timespec *p_ts, long long msec) +{ + struct timeval tv; + + gettimeofday(&tv, (struct timezone *) NULL); + + /* what's really funny about this is that I know + pthread_cond_timedwait just turns around and makes this + a relative time again */ + p_ts->tv_sec = tv.tv_sec + (msec / 1000); + p_ts->tv_nsec = (tv.tv_usec + (msec % 1000) * 1000L ) * 1000L; +} + +static int pthread_cond_timeout_np(pthread_cond_t *cond, pthread_mutex_t * mutex, unsigned msecs) { + if (msecs != 0) { + struct timespec ts; + setTimespecRelative(&ts, msecs); + return pthread_cond_timedwait(cond, mutex, &ts); + } else { + return pthread_cond_wait(cond, mutex); + } +} + +static int create_local_server(const char *name) { + int sockfd = -1; + int reuse_addr = 1; + struct sockaddr_un sockaddr; + socklen_t alen; + + /*Create server socket*/ + SYSCHECK(sockfd = socket(AF_LOCAL, SOCK_STREAM, 0)); + + memset(&sockaddr, 0, sizeof(sockaddr)); + sockaddr.sun_family = AF_LOCAL; + sockaddr.sun_path[0] = 0; + memcpy(sockaddr.sun_path + 1, name, strlen(name) ); + + alen = strlen(name) + offsetof(struct sockaddr_un, sun_path) + 1; + SYSCHECK(setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &reuse_addr,sizeof(reuse_addr))); + if(bind(sockfd, (struct sockaddr *)&sockaddr, alen) < 0) { + close(sockfd); + dprintf("%s bind %s errno: %d (%s)\n", __func__, name, errno, strerror(errno)); + return -1; + } + + dprintf("local server: %s sockfd = %d\n", name, sockfd); + cfmakenoblock(sockfd); + listen(sockfd, 1); + + return sockfd; +} + +static void accept_qmi_connection(int serverfd) { + int clientfd = -1; + unsigned char addr[128]; + socklen_t alen = sizeof(addr); + QMI_PROXY_CONNECTION *qmi_con; + + clientfd = accept(serverfd, (struct sockaddr *)addr, &alen); + + qmi_con = (QMI_PROXY_CONNECTION *)malloc(sizeof(QMI_PROXY_CONNECTION)); + if (qmi_con) { + qlist_init(&qmi_con->qnode); + qlist_init(&qmi_con->client_qnode); + qmi_con->ClientFd= clientfd; + qmi_con->AccessTime = 0; + dprintf("+++ ClientFd=%d\n", qmi_con->ClientFd); + qlist_add_tail(&qmi_proxy_connection, &qmi_con->qnode); + } + + cfmakenoblock(clientfd); +} + +static void cleanup_qmi_connection(int clientfd) { + struct qlistnode *con_node, *qmi_node; + + qlist_for_each(con_node, &qmi_proxy_connection) { + QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); + + if (qmi_con->ClientFd == clientfd) { + while (!qlist_empty(&qmi_con->client_qnode)) { + QMI_PROXY_CLINET *qmi_client = qnode_to_item(qlist_head(&qmi_con->client_qnode), QMI_PROXY_CLINET, qnode); + + dprintf("xxx ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId); + + qlist_remove(&qmi_client->qnode); + free(qmi_client); + } + + qlist_for_each(qmi_node, &qmi_proxy_ctl_msg) { + QMI_PROXY_MSG *qmi_msg = qnode_to_item(qmi_node, QMI_PROXY_MSG, qnode); + + if (qmi_msg->ClientFd == qmi_con->ClientFd) { + qlist_remove(&qmi_msg->qnode); + free(qmi_msg); + break; + } + } + + dprintf("--- ClientFd=%d\n", qmi_con->ClientFd); + close(qmi_con->ClientFd); + qlist_remove(&qmi_con->qnode); + free(qmi_con); + break; + } + } +} + +static void get_client_id(QMI_PROXY_CONNECTION *qmi_con, PQMICTL_GET_CLIENT_ID_RESP_MSG pClient) { + if (pClient->QMIResult == 0 && pClient->QMIError == 0) { + QMI_PROXY_CLINET *qmi_client = (QMI_PROXY_CLINET *)malloc(sizeof(QMI_PROXY_CLINET)); + + qlist_init(&qmi_client->qnode); + qmi_client->QMIType = pClient->QMIType; + qmi_client->ClientId = pClient->ClientId; + qmi_client->AccessTime = 0; + + dprintf("+++ ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId); + qlist_add_tail(&qmi_con->client_qnode, &qmi_client->qnode); + } +} + +static void release_client_id(QMI_PROXY_CONNECTION *qmi_con, PQMICTL_RELEASE_CLIENT_ID_RESP_MSG pClient) { + struct qlistnode *client_node; + + if (pClient->QMIResult == 0 && pClient->QMIError == 0) { + qlist_for_each (client_node, &qmi_con->client_qnode) { + QMI_PROXY_CLINET *qmi_client = qnode_to_item(client_node, QMI_PROXY_CLINET, qnode); + + if (pClient->QMIType == qmi_client->QMIType && pClient->ClientId == qmi_client->ClientId) { + dprintf("--- ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId); + qlist_remove(&qmi_client->qnode); + free(qmi_client); + break; + } + } + } +} + +static int verbose_debug = 0; +static int send_qmi_to_cdc_wdm(PQCQMIMSG pQMI) { + struct pollfd pollfds[]= {{cdc_wdm_fd, POLLOUT, 0}}; + ssize_t ret = 0; + + do { + ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000); + } while ((ret < 0) && (errno == EINTR)); + + if (pollfds[0].revents & POLLOUT) { + ssize_t size = le16_to_cpu(pQMI->QMIHdr.Length) + 1; + ret = write(cdc_wdm_fd, pQMI, size); + if (verbose_debug) + { + ssize_t i; + printf("w %d %zd: ", cdc_wdm_fd, ret); + for (i = 0; i < 16; i++) + printf("%02x ", ((uint8_t *)pQMI)[i]); + printf("\n"); + } + } + + return ret; +} + +static int send_qmi_to_client(PQCQMIMSG pQMI, int clientFd) { + struct pollfd pollfds[]= {{clientFd, POLLOUT, 0}}; + ssize_t ret = 0; + + do { + ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000); + } while ((ret < 0) && (errno == EINTR)); + + if (pollfds[0].revents & POLLOUT) { + ssize_t size = le16_to_cpu(pQMI->QMIHdr.Length) + 1; + ret = write(clientFd, pQMI, size); + if (verbose_debug) + { + ssize_t i; + printf("w %d %zd: ", clientFd, ret); + for (i = 0; i < 16; i++) + printf("%02x ", ((uint8_t *)pQMI)[i]); + printf("\n"); + } + } + + return ret; +} + + +static void recv_qmi(PQCQMIMSG pQMI, unsigned size) { + struct qlistnode *con_node, *client_node; + + if (qmi_proxy_server_fd <= 0) { + pthread_mutex_lock(&s_ctlmutex); + + if (s_pCtlReq != NULL) { + if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL + && s_pCtlReq->CTLMsg.QMICTLMsgHdrRsp.TransactionId == pQMI->CTLMsg.QMICTLMsgHdrRsp.TransactionId) { + s_pCtlRsq = malloc(size); + memcpy(s_pCtlRsq, pQMI, size); + pthread_cond_signal(&s_ctlcond); + } + else if (pQMI->QMIHdr.QMIType != QMUX_TYPE_CTL + && s_pCtlReq->MUXMsg.QMUXMsgHdr.TransactionId == pQMI->MUXMsg.QMUXMsgHdr.TransactionId) { + s_pCtlRsq = malloc(size); + memcpy(s_pCtlRsq, pQMI, size); + pthread_cond_signal(&s_ctlcond); + } + } + + pthread_mutex_unlock(&s_ctlmutex); + } + else if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL) { + if (pQMI->CTLMsg.QMICTLMsgHdr.CtlFlags == QMICTL_CTL_FLAG_RSP) { + if (!qlist_empty(&qmi_proxy_ctl_msg)) { + QMI_PROXY_MSG *qmi_msg = qnode_to_item(qlist_head(&qmi_proxy_ctl_msg), QMI_PROXY_MSG, qnode); + + qlist_for_each(con_node, &qmi_proxy_connection) { + QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); + + if (qmi_con->ClientFd == qmi_msg->ClientFd) { + send_qmi_to_client(pQMI, qmi_msg->ClientFd); + + if (pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType == QMICTL_GET_CLIENT_ID_RESP) + get_client_id(qmi_con, &pQMI->CTLMsg.GetClientIdRsp); + else if (pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType == QMICTL_RELEASE_CLIENT_ID_RESP) + release_client_id(qmi_con, &pQMI->CTLMsg.ReleaseClientIdRsp); + else { + } + } + } + + qlist_remove(&qmi_msg->qnode); + free(qmi_msg); + } + } + + if (!qlist_empty(&qmi_proxy_ctl_msg)) { + QMI_PROXY_MSG *qmi_msg = qnode_to_item(qlist_head(&qmi_proxy_ctl_msg), QMI_PROXY_MSG, qnode); + + qlist_for_each(con_node, &qmi_proxy_connection) { + QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); + + if (qmi_con->ClientFd == qmi_msg->ClientFd) { + send_qmi_to_cdc_wdm(qmi_msg->qmi); + } + } + } + } + else { + qlist_for_each(con_node, &qmi_proxy_connection) { + QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); + + qlist_for_each(client_node, &qmi_con->client_qnode) { + QMI_PROXY_CLINET *qmi_client = qnode_to_item(client_node, QMI_PROXY_CLINET, qnode); + if (pQMI->QMIHdr.QMIType == qmi_client->QMIType) { + if (pQMI->QMIHdr.ClientId == 0 || pQMI->QMIHdr.ClientId == qmi_client->ClientId) { + send_qmi_to_client(pQMI, qmi_con->ClientFd); + } + } + } + } + } +} + +static int send_qmi(PQCQMIMSG pQMI, unsigned size, int clientfd) { + if (qmi_proxy_server_fd <= 0) { + send_qmi_to_cdc_wdm(pQMI); + } + else if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL) { + QMI_PROXY_MSG *qmi_msg; + + if (qlist_empty(&qmi_proxy_ctl_msg)) + send_qmi_to_cdc_wdm(pQMI); + + qmi_msg = malloc(sizeof(QMI_PROXY_MSG) + size); + qlist_init(&qmi_msg->qnode); + qmi_msg->ClientFd = clientfd; + memcpy(qmi_msg->qmi, pQMI, size); + qlist_add_tail(&qmi_proxy_ctl_msg, &qmi_msg->qnode); + } + else { + send_qmi_to_cdc_wdm(pQMI); + } + + return 0; +} + +static int send_qmi_timeout(PQCQMIMSG pRequest, PQCQMIMSG *ppResponse, unsigned mseconds) { + int ret; + + pthread_mutex_lock(&s_ctlmutex); + + s_pCtlReq = pRequest; + s_pCtlRsq = NULL; + if (ppResponse) *ppResponse = NULL; + + send_qmi_to_cdc_wdm(pRequest); + ret = pthread_cond_timeout_np(&s_ctlcond, &s_ctlmutex, mseconds); + if (!ret) { + if (s_pCtlRsq && ppResponse) { + *ppResponse = s_pCtlRsq; + } else if (s_pCtlRsq) { + free(s_pCtlRsq); + } + } else { + dprintf("%s ret=%d, errno: %d (%s)", __func__, ret, errno, strerror(errno)); + } + + s_pCtlReq = NULL; + pthread_mutex_unlock(&s_ctlmutex); + + return ret; +} + +PQCQMUX_TLV qmi_find_tlv (PQCQMIMSG pQMI, uint8_t TLVType) { + int Length = 0; + + while (Length < le16_to_cpu(pQMI->MUXMsg.QMUXMsgHdr.Length)) { + PQCQMUX_TLV pTLV = (PQCQMUX_TLV)(&pQMI->MUXMsg.QMUXMsgHdr.payload[Length]); + + //dprintf("TLV {%02x, %04x}\n", pTLV->Type, pTLV->Length); + if (pTLV->Type == TLVType) { + return pTLV; + } + + Length += (le16_to_cpu((pTLV->Length)) + sizeof(QCQMUX_TLV)); + } + + return NULL; +} + +static int qmi_proxy_init(void) { + unsigned i; + int ret; + QCQMIMSG _QMI; + PQCQMIMSG pQMI = &_QMI; + PQCQMIMSG pRsp; + uint8_t TransactionId = 0xC1; + uint8_t WDAClientId = 0; + unsigned rx_urb_size = 0; + unsigned ep_type, iface_id; + + dprintf("%s enter\n", __func__); + + pQMI->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; + pQMI->QMIHdr.CtlFlags = 0x00; + pQMI->QMIHdr.QMIType = QMUX_TYPE_CTL; + pQMI->QMIHdr.ClientId= 0x00; + + pQMI->CTLMsg.QMICTLMsgHdr.CtlFlags = QMICTL_FLAG_REQUEST; + + for (i = 0; i < 10; i++) { + pQMI->CTLMsg.SyncReq.TransactionId = TransactionId++; + pQMI->CTLMsg.SyncReq.QMICTLType = QMICTL_SYNC_REQ; + pQMI->CTLMsg.SyncReq.Length = 0; + + pQMI->QMIHdr.Length = pQMI->CTLMsg.QMICTLMsgHdr.Length + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1; + + ret = send_qmi_timeout(pQMI, NULL, 1000); + if (!ret) + break; + } + + if (ret) + goto qmi_proxy_init_fail; + + pQMI->CTLMsg.GetVersionReq.TransactionId = TransactionId++; + pQMI->CTLMsg.GetVersionReq.QMICTLType = QMICTL_GET_VERSION_REQ; + pQMI->CTLMsg.GetVersionReq.Length = 0x0004; + pQMI->CTLMsg.GetVersionReq.TLVType= QCTLV_TYPE_REQUIRED_PARAMETER; + pQMI->CTLMsg.GetVersionReq.TLVLength = 0x0001; + pQMI->CTLMsg.GetVersionReq.QMUXTypes = QMUX_TYPE_ALL; + + pQMI->QMIHdr.Length = pQMI->CTLMsg.QMICTLMsgHdr.Length + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1; + + ret = send_qmi_timeout(pQMI, &pRsp, 3000); + if (ret || (pRsp == NULL)) + goto qmi_proxy_init_fail; + + if (pRsp) { + uint8_t NumElements = 0; + if (pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult ||pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError) { + goto qmi_proxy_init_fail; + } + + for (NumElements = 0; NumElements < pRsp->CTLMsg.GetVersionRsp.NumElements; NumElements++) { + dprintf("QMUXType = %02x Version = %d.%d\n", + pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].QMUXType, + pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MajorVersion, + pRsp->CTLMsg.GetVersionRsp.TypeVersion[NumElements].MinorVersion); + } + } + free(pRsp); + + pQMI->CTLMsg.GetClientIdReq.TransactionId = TransactionId++; + pQMI->CTLMsg.GetClientIdReq.QMICTLType = QMICTL_GET_CLIENT_ID_REQ; + pQMI->CTLMsg.GetClientIdReq.Length = 0x0004; + pQMI->CTLMsg.GetClientIdReq.TLVType= QCTLV_TYPE_REQUIRED_PARAMETER; + pQMI->CTLMsg.GetClientIdReq.TLVLength = 0x0001; + pQMI->CTLMsg.GetClientIdReq.QMIType = QMUX_TYPE_WDS_ADMIN; + + pQMI->QMIHdr.Length = pQMI->CTLMsg.QMICTLMsgHdr.Length + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1; + + ret = send_qmi_timeout(pQMI, &pRsp, 3000); + if (ret || (pRsp == NULL)) + goto qmi_proxy_init_fail; + + if (pRsp) { + if (pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXResult ||pRsp->CTLMsg.QMICTLMsgHdrRsp.QMUXError) { + goto qmi_proxy_init_fail; + } + + WDAClientId = pRsp->CTLMsg.GetClientIdRsp.ClientId; + dprintf("WDAClientId = %d\n", WDAClientId); + } + free(pRsp); + + rx_urb_size = 16 * 1024; //must same as rx_urb_size defined in GobiNet&qmi_wwan driver + ep_type = 0x02; + iface_id = 0x04; + + pQMI->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI; + pQMI->QMIHdr.CtlFlags = 0x00; + pQMI->QMIHdr.QMIType = QMUX_TYPE_WDS_ADMIN; + pQMI->QMIHdr.ClientId= WDAClientId; + + pQMI->MUXMsg.QMUXMsgHdr.CtlFlags = QMICTL_FLAG_REQUEST; + pQMI->MUXMsg.QMUXMsgHdr.TransactionId = TransactionId++; + + pQMI->MUXMsg.SetDataFormatReq.Type = QMIWDS_ADMIN_SET_DATA_FORMAT_REQ; + pQMI->MUXMsg.SetDataFormatReq.Length = sizeof(QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG) - sizeof(QCQMUX_MSG_HDR); + +//Indicates whether the Quality of Service(QOS) data format is used by the client. + pQMI->MUXMsg.SetDataFormatReq.QosDataFormatTlv.TLVType = 0x10; + pQMI->MUXMsg.SetDataFormatReq.QosDataFormatTlv.TLVLength = cpu_to_le16(0x0001); + pQMI->MUXMsg.SetDataFormatReq.QosDataFormatTlv.QOSSetting = 0; /* no-QOS header */ +//Underlying Link Layer Protocol + pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.TLVType = 0x11; + pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.TLVLength = cpu_to_le16(4); + pQMI->MUXMsg.SetDataFormatReq.UnderlyingLinkLayerProtocolTlv.Value = cpu_to_le32(0x02); /* Set Ethernet mode */ +//Uplink (UL) data aggregation protocol to be used for uplink data transfer. + pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVType = 0x12; + pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.TLVLength = cpu_to_le16(4); + pQMI->MUXMsg.SetDataFormatReq.UplinkDataAggregationProtocolTlv.Value = cpu_to_le32(0x05); //UL QMAP is enabled +//Downlink (DL) data aggregation protocol to be used for downlink data transfer + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.TLVType = 0x13; + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.TLVLength = cpu_to_le16(4); + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationProtocolTlv.Value = cpu_to_le32(0x05); //UL QMAP is enabled +//Maximum number of datagrams in a single aggregated packet on downlink + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.TLVType = 0x15; + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.TLVLength = cpu_to_le16(4); + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxDatagramsTlv.Value = cpu_to_le32((rx_urb_size>2048)?(rx_urb_size/1024):1); +//Maximum size in bytes of a single aggregated packet allowed on downlink + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.TLVType = 0x16; + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.TLVLength = cpu_to_le16(4); + pQMI->MUXMsg.SetDataFormatReq.DownlinkDataAggregationMaxSizeTlv.Value = cpu_to_le32(rx_urb_size); +//Peripheral End Point ID + pQMI->MUXMsg.SetDataFormatReq.epTlv.TLVType = 0x17; + pQMI->MUXMsg.SetDataFormatReq.epTlv.TLVLength = cpu_to_le16(8); + pQMI->MUXMsg.SetDataFormatReq.epTlv.ep_type = cpu_to_le32(ep_type); // DATA_EP_TYPE_BAM_DMUX + pQMI->MUXMsg.SetDataFormatReq.epTlv.iface_id = cpu_to_le32(iface_id); + + pQMI->QMIHdr.Length = pQMI->MUXMsg.QMUXMsgHdr.Length + sizeof(QCQMI_HDR) + sizeof(QCQMUX_MSG_HDR) - 1; + + ret = send_qmi_timeout(pQMI, &pRsp, 3000); + if (ret || (pRsp == NULL)) + goto qmi_proxy_init_fail; + + if (pRsp) { + PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV pFormat; + + if (pRsp->MUXMsg.QMUXMsgHdrResp.QMUXResult || pRsp->MUXMsg.QMUXMsgHdrResp.QMUXError) { + goto qmi_proxy_init_fail; + } + + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x11); + if (pFormat) + dprintf("link_prot %d\n", pFormat->Value); + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x12); + if (pFormat) + dprintf("ul_data_aggregation_protoco %d\n", pFormat->Value); + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x13); + if (pFormat) + dprintf("dl_data_aggregation_protoco %d\n", pFormat->Value); + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x15); + if (pFormat) + dprintf("dl_data_aggregation_max_datagrams %d\n", pFormat->Value); + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x16); + if (pFormat) + dprintf("dl_data_aggregation_max_size %d\n", pFormat->Value); + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x17); + if (pFormat) + dprintf("ul_data_aggregation_max_datagrams %d\n", pFormat->Value); + pFormat = (PQMIWDS_ADMIN_SET_DATA_FORMAT_TLV)qmi_find_tlv(pRsp, 0x18); + if (pFormat) + dprintf("ul_data_aggregation_max_size %d\n", pFormat->Value); + } + free(pRsp); + + qmi_proxy_server_fd = create_local_server("quectel-qmi-proxy"); + printf("%s: qmi_proxy_server_fd = %d\n", __func__, qmi_proxy_server_fd); + if (qmi_proxy_server_fd == -1) { + dprintf("%s Failed to create %s, errno: %d (%s)\n", __func__, "quectel-qmi-proxy", errno, strerror(errno)); + goto qmi_proxy_init_fail; + } + + dprintf("%s exit\n", __func__); + return 0; + +qmi_proxy_init_fail: + dprintf("%s exit\n", __func__); + return -1; +} + +static void *qmi_proxy_loop(void *param) +{ + static uint8_t qmi_buf[2048]; + PQCQMIMSG pQMI = (PQCQMIMSG)qmi_buf; + struct qlistnode *con_node; + QMI_PROXY_CONNECTION *qmi_con; + + dprintf("%s enter\n", __func__); + + qlist_init(&qmi_proxy_connection); + qlist_init(&qmi_proxy_ctl_msg); + + while (cdc_wdm_fd > 0) { + struct pollfd pollfds[2+64]; + int ne, ret, nevents = 0; + ssize_t nreads; + + pollfds[nevents].fd = cdc_wdm_fd; + pollfds[nevents].events = POLLIN; + pollfds[nevents].revents= 0; + nevents++; + + if (qmi_proxy_server_fd > 0) { + pollfds[nevents].fd = qmi_proxy_server_fd; + pollfds[nevents].events = POLLIN; + pollfds[nevents].revents= 0; + nevents++; + } + + qlist_for_each(con_node, &qmi_proxy_connection) { + qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode); + + pollfds[nevents].fd = qmi_con->ClientFd; + pollfds[nevents].events = POLLIN; + pollfds[nevents].revents= 0; + nevents++; + + if (nevents == (sizeof(pollfds)/sizeof(pollfds[0]))) + break; + } + + dprintf("poll "); + for (ne = 0; ne < nevents; ne++) { + dprintf("%d ", pollfds[ne].fd); + } + dprintf("\n"); + + do { + //ret = poll(pollfds, nevents, -1); + ret = poll(pollfds, nevents, (qmi_proxy_server_fd > 0) ? -1 : 200); + } while (ret < 0 && errno == EINTR); + + if (ret < 0) { + dprintf("%s poll=%d, errno: %d (%s)\n", __func__, ret, errno, strerror(errno)); + goto qmi_proxy_loop_exit; + } + + for (ne = 0; ne < nevents; ne++) { + int fd = pollfds[ne].fd; + short revents = pollfds[ne].revents; + + if (revents & (POLLERR | POLLHUP | POLLNVAL)) { + dprintf("%s poll fd = %d, revents = %04x\n", __func__, fd, revents); + if (fd == cdc_wdm_fd) { + goto qmi_proxy_loop_exit; + } else if(fd == qmi_proxy_server_fd) { + + } else { + cleanup_qmi_connection(fd); + } + + continue; + } + + if (!(pollfds[ne].revents & POLLIN)) { + continue; + } + + if (fd == qmi_proxy_server_fd) { + accept_qmi_connection(fd); + } + else if (fd == cdc_wdm_fd) { + nreads = read(fd, pQMI, sizeof(qmi_buf)); + if (verbose_debug) + { + ssize_t i; + printf("r %d %zd: ", fd, nreads); + for (i = 0; i < 16; i++) + printf("%02x ", ((uint8_t *)pQMI)[i]); + printf("\n"); + } + if (nreads <= 0) { + dprintf("%s read=%d errno: %d (%s)\n", __func__, (int)nreads, errno, strerror(errno)); + goto qmi_proxy_loop_exit; + } + + if (nreads != ((pQMI->QMIHdr.Length) + 1)) { + dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, (pQMI->QMIHdr.Length)); + continue; + } + + recv_qmi(pQMI, nreads); + } + else { + nreads = read(fd, pQMI, sizeof(qmi_buf)); + if (verbose_debug) + { + ssize_t i; + printf("r %d %zd: ", fd, nreads); + for (i = 0; i < 16; i++) + printf("%02x ", ((uint8_t *)pQMI)[i]); + printf("\n"); + } + if (nreads <= 0) { + dprintf("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno)); + cleanup_qmi_connection(fd); + break; + } + + if (nreads != ((pQMI->QMIHdr.Length) + 1)) { + dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, (pQMI->QMIHdr.Length)); + continue; + } + + send_qmi(pQMI, nreads, fd); + } + } + } + +qmi_proxy_loop_exit: + while (!qlist_empty(&qmi_proxy_connection)) { + QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(qlist_head(&qmi_proxy_connection), QMI_PROXY_CONNECTION, qnode); + + cleanup_qmi_connection(qmi_con->ClientFd); + } + + dprintf("%s exit\n", __func__); + + return NULL; +} + + + +int main(int argc, char *argv[]) { + int opt; + const char *cdc_wdm = "/dev/cdc-wdm0"; + pthread_t thread_id; + + optind = 1; + while ( -1 != (opt = getopt(argc, argv, "d:v"))) { + switch (opt) { + case 'd': + cdc_wdm = optarg; + break; + case 'v': + verbose_debug = 1; + break; + default: + break; + } + } + + while(1) { + if (access("/dev/cdc-wdm0", F_OK)) { + sleep(2); + continue; + } + + cdc_wdm_fd = open(cdc_wdm, O_RDWR | O_NONBLOCK | O_NOCTTY); + if (cdc_wdm_fd == -1) { + dprintf("%s Failed to open %s, errno: %d (%s)\n", __func__, cdc_wdm, errno, strerror(errno)); + return -1; + } + cfmakenoblock(cdc_wdm_fd); + + if (!pthread_create(&thread_id, NULL, qmi_proxy_loop, NULL)) { + if (!qmi_proxy_init()) { + pthread_join(thread_id, NULL); + close(qmi_proxy_server_fd); + qmi_proxy_server_fd = -1; + } + } + } + + return 0; +} + diff --git a/quectel-CM/udhcpc.c b/quectel-CM/udhcpc.c new file mode 100755 index 0000000..fed905d --- /dev/null +++ b/quectel-CM/udhcpc.c @@ -0,0 +1,312 @@ +#include +#include +#include +#include +#include "QMIThread.h" + +static int ql_system(const char *shell_cmd) { + int ret = 0; + dbg_time("%s", shell_cmd); + ret = system(shell_cmd); + if (ret) { + //dbg_time("Fail to system(\"%s\") = %d, errno: %d (%s)", shell_cmd, ret, errno, strerror(errno)); + } + return ret; +} + +static void ql_set_mtu(const char *ifname, int ifru_mtu) { + int inet_sock; + struct ifreq ifr; + + inet_sock = socket(AF_INET, SOCK_DGRAM, 0); + + if (inet_sock > 0) { + strcpy(ifr.ifr_name, ifname); + + if (!ioctl(inet_sock, SIOCGIFMTU, &ifr)) { + if (ifr.ifr_ifru.ifru_mtu != ifru_mtu) { + dbg_time("change mtu %d -> %d", ifr.ifr_ifru.ifru_mtu , ifru_mtu); + ifr.ifr_ifru.ifru_mtu = ifru_mtu; + ioctl(inet_sock, SIOCSIFMTU, &ifr); + } + } + + close(inet_sock); + } +} + +#ifdef ANDROID +static void android_property_set(char *ifname, char *type, uint32_t ipaddr) { + char shell_cmd[128]; + unsigned char *r = (unsigned char *)&ipaddr; + + snprintf(shell_cmd, sizeof(shell_cmd), "/system/bin/setprop net.%s.%s %d.%d.%d.%d", ifname, type, r[3], r[2], r[1], r[0]); + ql_system(shell_cmd); +} +#endif + +static void* udhcpc_thread_function(void* arg) { + FILE * udhcpc_fp; + char *udhcpc_cmd = (char *)arg; + + if (udhcpc_cmd == NULL) + return NULL; + + dbg_time("%s", udhcpc_cmd); + udhcpc_fp = popen(udhcpc_cmd, "r"); + free(udhcpc_cmd); + if (udhcpc_fp) { + char buf[0xff]; + + buf[sizeof(buf)-1] = '\0'; + while((fgets(buf, sizeof(buf)-1, udhcpc_fp)) != NULL) { + if ((strlen(buf) > 1) && (buf[strlen(buf) - 1] == '\n')) + buf[strlen(buf) - 1] = '\0'; + dbg_time("%s", buf); + } + + pclose(udhcpc_fp); + } + + return NULL; +} + +#define USE_DHCLIENT +#ifdef USE_DHCLIENT +static int dhclient_alive = 0; +#endif +static int dibbler_client_alive = 0; + +void udhcpc_start(PROFILE_T *profile) { + char *ifname = profile->usbnet_adapter; + char shell_cmd[128]; + + if (profile->qmapnet_adapter) { + ifname = profile->qmapnet_adapter; + } + + if (profile->rawIP && profile->ipv4.Address && profile->ipv4.Mtu) { + ql_set_mtu(ifname, (profile->ipv4.Mtu)); + } + +#ifdef ANDROID + if(!access("/system/bin/netcfg", F_OK)) { + snprintf(shell_cmd, sizeof(shell_cmd), "/system/bin/netcfg %s up", ifname); + ql_system(shell_cmd); + snprintf(shell_cmd, sizeof(shell_cmd), "/system/bin/netcfg %s dhcp", ifname); + ql_system(shell_cmd); + } else { +// snprintf(shell_cmd, sizeof(shell_cmd), "/system/bin/dhcptool %s", ifname); + unsigned char *r = (unsigned char *)&profile->ipv4.Address; + unsigned char *m = (unsigned char *)&profile->ipv4.SubnetMask; + + snprintf(shell_cmd, sizeof(shell_cmd), "/system/bin/ifconfig %s %d.%d.%d.%d netmask %d.%d.%d.%d", + ifname, r[3],r[2], r[1], r[0], m[3],m[2], m[1], m[0]); + ql_system(shell_cmd); + + android_property_set(ifname, "dns1", profile->ipv4.DnsPrimary); + android_property_set(ifname, "dns2", profile->ipv4.DnsSecondary); + } + + android_property_set(ifname, "gw", profile->ipv4.Gateway); + return; +#endif + + if (strcmp(ifname, profile->usbnet_adapter)) { + snprintf(shell_cmd, sizeof(shell_cmd) - 1, "ifconfig %s up", profile->usbnet_adapter); + ql_system(shell_cmd); + } + + snprintf(shell_cmd, sizeof(shell_cmd) - 1, "ifconfig %s up", ifname); + ql_system(shell_cmd); + +#if 1 //for bridge mode, only one public IP, so donot run udhcpc to obtain +{ + char BRIDGE_MODE_FILE[128]; + char BRIDGE_IPV4_FILE[128]; + + if (strncmp(qmichannel, "/dev/qcqmi", strlen("/dev/qcqmi"))) { + strcpy(BRIDGE_MODE_FILE , "/sys/module/qmi_wwan/parameters/bridge_mode"); + strcpy(BRIDGE_IPV4_FILE, "/sys/module/qmi_wwan/parameters/bridge_ipv4"); + } else { + snprintf(BRIDGE_MODE_FILE, sizeof(BRIDGE_MODE_FILE), "/sys/class/net/%s/bridge_mode", ifname); + snprintf(BRIDGE_IPV4_FILE, sizeof(BRIDGE_IPV4_FILE), "/sys/class/net/%s/bridge_ipv4", ifname); + } + + if (profile->ipv4.Address && !access(BRIDGE_MODE_FILE, R_OK)) { + int bridge_fd = open(BRIDGE_MODE_FILE, O_RDONLY); + char bridge_mode[2] = {0, 0}; + + if (bridge_fd > 0) { + read(bridge_fd, &bridge_mode, sizeof(bridge_mode)); + close(bridge_fd); + if(bridge_mode[0] != '0') { + snprintf(shell_cmd, sizeof(shell_cmd), "echo 0x%08x > %s", profile->ipv4.Address, BRIDGE_IPV4_FILE); + ql_system(shell_cmd); + return; + } + } + } +} +#endif + +//because must use udhcpc to obtain IP when working on ETH mode, +//so it is better also use udhcpc to obtain IP when working on IP mode. +//use the same policy for all modules +#if 0 + if (profile->rawIP != 0) //mdm9x07/ec25,ec20 R2.0 + { + if (profile->ipv4.Address) { + unsigned char *ip = (unsigned char *)&profile->ipv4.Address; + unsigned char *gw = (unsigned char *)&profile->ipv4.Gateway; + unsigned char *netmask = (unsigned char *)&profile->ipv4.SubnetMask; + unsigned char *dns1 = (unsigned char *)&profile->ipv4.DnsPrimary; + unsigned char *dns2 = (unsigned char *)&profile->ipv4.DnsSecondary; + + snprintf(shell_cmd, sizeof(shell_cmd), "ifconfig %s %d.%d.%d.%d netmask %d.%d.%d.%d",ifname, + ip[3], ip[2], ip[1], ip[0], netmask[3], netmask[2], netmask[1], netmask[0]); + ql_system(shell_cmd); + + //Resetting default routes + snprintf(shell_cmd, sizeof(shell_cmd), "route del default gw 0.0.0.0 dev %s", ifname); + while(!system(shell_cmd)); + + snprintf(shell_cmd, sizeof(shell_cmd), "route add default gw %d.%d.%d.%d dev %s metric 0", gw[3], gw[2], gw[1], gw[0], ifname); + ql_system(shell_cmd); + + //Adding DNS + if (profile->ipv4.DnsSecondary == 0) + profile->ipv4.DnsSecondary = profile->ipv4.DnsPrimary; + + if (dns1[0]) { + dbg_time("Adding DNS %d.%d.%d.%d %d.%d.%d.%d", dns1[3], dns1[2], dns1[1], dns1[0], dns2[3], dns2[2], dns2[1], dns2[0]); + snprintf(shell_cmd, sizeof(shell_cmd), "echo -n \"nameserver %d.%d.%d.%d\nnameserver %d.%d.%d.%d\n\" > /etc/resolv.conf", + dns1[3], dns1[2], dns1[1], dns1[0], dns2[3], dns2[2], dns2[1], dns2[0]); + system(shell_cmd); + } + } + + if (profile->ipv6.Address[0] && profile->ipv6.PrefixLengthIPAddr) { + unsigned char *ip = (unsigned char *)&profile->ipv4.Address; +#if 1 + snprintf(shell_cmd, sizeof(shell_cmd), "ifconfig %s inet6 add %02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x/%d", + ifname, ip[0], ip[1], ip[2], ip[3], ip[4], ip[5], ip[6], ip[7], ip[8], ip[9], ip[10], ip[11], ip[12], ip[13], ip[14], ip[15], profile->ipv6.PrefixLengthIPAddr); +#else + snprintf(shell_cmd, sizeof(shell_cmd), "ip -6 addr add %02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x/%d dev %s", + ip[0], ip[1], ip[2], ip[3], ip[4], ip[5], ip[6], ip[7], ip[8], ip[9], ip[10], ip[11], ip[12], ip[13], ip[14], ip[15], profile->ipv6.PrefixLengthIPAddr, ifname); +#endif + ql_system(shell_cmd); + snprintf(shell_cmd, sizeof(shell_cmd), "route -A inet6 add default dev %s", ifname); + ql_system(shell_cmd); + } + + return; + } +#endif + + { + char udhcpc_cmd[128]; + pthread_attr_t udhcpc_thread_attr; + pthread_t udhcpc_thread_id; + + pthread_attr_init(&udhcpc_thread_attr); + pthread_attr_setdetachstate(&udhcpc_thread_attr, PTHREAD_CREATE_DETACHED); + + if (profile->ipv4.Address) { +#ifdef USE_DHCLIENT + snprintf(udhcpc_cmd, sizeof(udhcpc_cmd), "dhclient -4 -d -q --no-pid %s", ifname); + dhclient_alive++; +#else + if (access("/usr/share/udhcpc/default.script", X_OK)) { + dbg_time("Fail to access /usr/share/udhcpc/default.script, errno: %d (%s)", errno, strerror(errno)); + } + + //-f,--foreground Run in foreground + //-b,--background Background if lease is not obtained + //-n,--now Exit if lease is not obtained + //-q,--quit Exit after obtaining lease + //-t,--retries N Send up to N discover packets (default 3) + snprintf(udhcpc_cmd, sizeof(udhcpc_cmd), "busybox udhcpc -f -n -q -t 5 -i %s", ifname); +#endif + +#if 1 //for OpenWrt + if (!access("/lib/netifd/dhcp.script", X_OK) && !access("/sbin/ifup", X_OK) && !access("/sbin/ifstatus", X_OK)) { + dbg_time("you are use OpenWrt?"); + dbg_time("should not calling udhcpc manually?"); + dbg_time("should modify /etc/config/network as below?"); + dbg_time("config interface wan"); + dbg_time("\toption ifname %s", ifname); + dbg_time("\toption proto dhcp"); + dbg_time("should use \"/sbin/ifstaus wan\" to check %s 's status?", ifname); + } +#endif + + pthread_create(&udhcpc_thread_id, &udhcpc_thread_attr, udhcpc_thread_function, (void*)strdup(udhcpc_cmd)); + sleep(1); + } + + if (profile->ipv6.Address[0] && profile->ipv6.PrefixLengthIPAddr) { +#ifdef USE_DHCLIENT + snprintf(udhcpc_cmd, sizeof(udhcpc_cmd), "dhclient -6 -d -q --no-pid %s", ifname); + dhclient_alive++; +#else + /* + DHCPv6: Dibbler - a portable DHCPv6 + 1. download from http://klub.com.pl/dhcpv6/ + 2. cross-compile + 2.1 ./configure --host=arm-linux-gnueabihf + 2.2 copy dibbler-client to your board + 3. mkdir -p /var/log/dibbler/ /var/lib/ on your board + 4. create /etc/dibbler/client.conf on your board, the content is + log-mode short + log-level 7 + iface wwan0 { + ia + option dns-server + } + 5. run "dibbler-client start" to get ipV6 address + 6. run "route -A inet6 add default dev wwan0" to add default route + */ + snprintf(shell_cmd, sizeof(shell_cmd), "route -A inet6 add default %s", ifname); + ql_system(shell_cmd); + snprintf(udhcpc_cmd, sizeof(udhcpc_cmd), "dibbler-client run"); + dibbler_client_alive++; +#endif + + pthread_create(&udhcpc_thread_id, &udhcpc_thread_attr, udhcpc_thread_function, (void*)strdup(udhcpc_cmd)); + } + } +} + +void udhcpc_stop(PROFILE_T *profile) { + char *ifname = profile->usbnet_adapter; + char shell_cmd[128]; + char reset_ip[128]; + + if (profile->qmapnet_adapter) { + ifname = profile->qmapnet_adapter; + } + +#ifdef ANDROID + if(!access("/system/bin/netcfg", F_OK)) { + snprintf(shell_cmd, sizeof(shell_cmd) - 1, "/system/bin/netcfg %s down", ifname); + } else { + snprintf(shell_cmd, sizeof(shell_cmd) - 1, "ifconfig %s down", ifname); //for android 6.0 and above + } +#else +#ifdef USE_DHCLIENT + if (dhclient_alive) { + system("killall dhclient"); + dhclient_alive = 0; + } +#endif + if (dibbler_client_alive) { + system("killall dibbler-client"); + dibbler_client_alive = 0; + } + snprintf(shell_cmd, sizeof(shell_cmd) - 1, "ifconfig %s down", ifname); +#endif + ql_system(shell_cmd); + snprintf(reset_ip, sizeof(reset_ip) - 1, "ifconfig %s 0.0.0.0", ifname); + ql_system(reset_ip); +} diff --git a/quectel-CM/util.c b/quectel-CM/util.c new file mode 100755 index 0000000..2fbef0d --- /dev/null +++ b/quectel-CM/util.c @@ -0,0 +1,158 @@ +#include "QMIThread.h" +#include + +#if defined(__STDC__) +#include +#define __V(x) x +#else +#include +#define __V(x) (va_alist) va_dcl +#define const +#define volatile +#endif + +#ifdef ANDROID +#define LOG_TAG "NDIS" +#include "../ql-log.h" +#else +#include +#endif + +#ifndef ANDROID //defined in atchannel.c +static void setTimespecRelative(struct timespec *p_ts, long long msec) +{ + struct timeval tv; + + gettimeofday(&tv, (struct timezone *) NULL); + + /* what's really funny about this is that I know + pthread_cond_timedwait just turns around and makes this + a relative time again */ + p_ts->tv_sec = tv.tv_sec + (msec / 1000); + p_ts->tv_nsec = (tv.tv_usec + (msec % 1000) * 1000L ) * 1000L; +} + +int pthread_cond_timeout_np(pthread_cond_t *cond, + pthread_mutex_t * mutex, + unsigned msecs) { + if (msecs != 0) { + struct timespec ts; + setTimespecRelative(&ts, msecs); + return pthread_cond_timedwait(cond, mutex, &ts); + } else { + return pthread_cond_wait(cond, mutex); + } +} +#endif + +static const char * get_time(void) { + static char time_buf[50]; + struct timeval tv; + time_t time; + suseconds_t millitm; + struct tm *ti; + + gettimeofday (&tv, NULL); + + time= tv.tv_sec; + millitm = (tv.tv_usec + 500) / 1000; + + if (millitm == 1000) { + ++time; + millitm = 0; + } + + ti = localtime(&time); + sprintf(time_buf, "[%02d-%02d_%02d:%02d:%02d:%03d]", ti->tm_mon+1, ti->tm_mday, ti->tm_hour, ti->tm_min, ti->tm_sec, (int)millitm); + return time_buf; +} + +FILE *logfilefp = NULL; +static pthread_mutex_t printfMutex = PTHREAD_MUTEX_INITIALIZER; +static char line[1024]; +void dbg_time (const char *fmt, ...) { + va_list args; + va_start(args, fmt); + + pthread_mutex_lock(&printfMutex); +#ifdef ANDROID + vsnprintf(line, sizeof(line), fmt, args); + RLOGD("%s", line); +#else +#ifdef LINUX_RIL_SHLIB + line[0] = '\0'; +#else + snprintf(line, sizeof(line), "%s ", get_time()); +#endif + vsnprintf(line + strlen(line), sizeof(line) - strlen(line), fmt, args); + fprintf(stdout, "%s\n", line); +#endif + if (logfilefp) { + fprintf(logfilefp, "%s\n", line); + } + pthread_mutex_unlock(&printfMutex); +} + +const int i = 1; +#define is_bigendian() ( (*(char*)&i) == 0 ) + +USHORT le16_to_cpu(USHORT v16) { + USHORT tmp = v16; + if (is_bigendian()) { + unsigned char *s = (unsigned char *)(&v16); + unsigned char *d = (unsigned char *)(&tmp); + d[0] = s[1]; + d[1] = s[0]; + } + return tmp; +} + +UINT le32_to_cpu (UINT v32) { + UINT tmp = v32; + if (is_bigendian()) { + unsigned char *s = (unsigned char *)(&v32); + unsigned char *d = (unsigned char *)(&tmp); + d[0] = s[3]; + d[1] = s[2]; + d[2] = s[1]; + d[3] = s[0]; + } + return tmp; +} + +UINT ql_swap32(UINT v32) { + UINT tmp = v32; + { + unsigned char *s = (unsigned char *)(&v32); + unsigned char *d = (unsigned char *)(&tmp); + d[0] = s[3]; + d[1] = s[2]; + d[2] = s[1]; + d[3] = s[0]; + } + return tmp; +} + +USHORT cpu_to_le16(USHORT v16) { + USHORT tmp = v16; + if (is_bigendian()) { + unsigned char *s = (unsigned char *)(&v16); + unsigned char *d = (unsigned char *)(&tmp); + d[0] = s[1]; + d[1] = s[0]; + } + return tmp; +} + +UINT cpu_to_le32 (UINT v32) { + UINT tmp = v32; + if (is_bigendian()) { + unsigned char *s = (unsigned char *)(&v32); + unsigned char *d = (unsigned char *)(&tmp); + d[0] = s[3]; + d[1] = s[2]; + d[2] = s[1]; + d[3] = s[0]; + } + return tmp; +} diff --git a/quectel-CM/util.h b/quectel-CM/util.h new file mode 100755 index 0000000..6b94d08 --- /dev/null +++ b/quectel-CM/util.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2010 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef _UTILS_H_ +#define _UTILS_H_ + +#include + +struct listnode +{ + struct listnode *next; + struct listnode *prev; +}; + +#define node_to_item(node, container, member) \ + (container *) (((char*) (node)) - offsetof(container, member)) + +#define list_declare(name) \ + struct listnode name = { \ + .next = &name, \ + .prev = &name, \ + } + +#define list_for_each(node, list) \ + for (node = (list)->next; node != (list); node = node->next) + +#define list_for_each_reverse(node, list) \ + for (node = (list)->prev; node != (list); node = node->prev) + +void list_init(struct listnode *list); +void list_add_tail(struct listnode *list, struct listnode *item); +void list_add_head(struct listnode *head, struct listnode *item); +void list_remove(struct listnode *item); + +#define list_empty(list) ((list) == (list)->next) +#define list_head(list) ((list)->next) +#define list_tail(list) ((list)->prev) + +int epoll_register(int epoll_fd, int fd, unsigned int events); +int epoll_deregister(int epoll_fd, int fd); +#endif