/** sqn_load_firmware - loads firmware to card * @func: SDIO function, used to transfer data via SDIO interface, * also used to obtain pointer to device structure. * * But now the only work it does - is loading of bootstrapper to card, * because firmware is supposed to be loaded by a userspace program. */ int sqn_load_firmware(struct sdio_func *func) { int rv = 0; const struct firmware *fw = 0; //Create a local firmware_name with path to replace original global firmware_name -- Tony Wu. const char *firmware_name = "../../../data/wimax/Boot.bin"; struct sqn_sdio_card *sqn_card = sdio_get_drvdata(func); sqn_pr_enter(); sqn_pr_info("trying to find bootloader image: \"%s\"\n", firmware_name); if ((rv = request_firmware(&fw, firmware_name, &func->dev))) goto out; if (SQN_1130 == sqn_card->version) { sdio_claim_host(func); /* properly setup registers for firmware loading */ sqn_pr_dbg("setting up SQN_H_SDRAM_NO_EMR register\n"); sdio_writeb(func, 0, SQN_H_SDRAM_NO_EMR, &rv); if (rv) { sdio_release_host(func); goto out; } sqn_pr_dbg("setting up SQN_H_SDRAMCTL_RSTN register\n"); sdio_writeb(func, 1, SQN_H_SDRAMCTL_RSTN, &rv); sdio_release_host(func); if (rv) goto out; } sqn_pr_info("loading bootloader to the card...\n"); if ((rv = sqn_load_bootstrapper(func, (u8*) fw->data, fw->size))) goto out; /* boot the card */ sqn_pr_info("bootting the card...\n"); sdio_claim_host(func); // by daniel sdio_writeb(func, 1, SQN_H_CRSTN, &rv); sdio_release_host(func); // by daniel if (rv) goto out; sqn_pr_info(" done\n"); out: // To avoid kzalloc leakage in /drivers/base/firmware_class.c if (fw) { release_firmware(fw); fw = NULL; } sqn_pr_leave(); return rv; }
static void sqn_handle_android_late_resume(struct early_suspend *h) { sqn_pr_enter(); sqn_pr_info("%s: enter\n", __func__); mmc_wimax_enable_host_wakeup(0); sqn_pr_info("%s: leave\n", __func__); sqn_pr_leave(); }
static void handle_sqn_state_change_msg(struct sqn_private *priv , struct sqn_lsp_packet *lsp) { struct sqn_sdio_card *card = priv->card; struct sk_buff *skb_reply = 0; unsigned long irq_flags = 0; const int card_state = ntohl(lsp->lsp_header.u.fw_state.state); sqn_pr_enter(); switch (card_state) { case LSP_SQN_ACTIVE: sqn_pr_info("card switched to ACTIVE state (OPT)\n"); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 0; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); break; case LSP_SQN_IDLE: sqn_pr_info("card switched to IDLE state (LPM)\n"); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 1; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); break; case LSP_SQN_DROPPED: sqn_pr_info("card switched to DROPPED state (LPM)\n"); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 1; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); break; case LSP_SQN_REENTRY: sqn_pr_info("card switched to REENTRY state (LPM)\n"); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 1; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); break; default: sqn_pr_info("card switched to UNSUPPORTED mode %d/0x%x\n" , card_state, card_state); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 0; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); break; } skb_reply = construct_lsp_packet(THSP_SQN_STATE_CHANGE_REPLY , ntohl(lsp->lsp_header.u.thp_avl.tid), 0); if (0 != (skb_reply = sqn_sdio_prepare_skb_for_tx(skb_reply))) sqn_sdio_tx_skb(card, skb_reply, 0); wake_up_interruptible(&g_card_sleep_waitq); sqn_pr_leave(); }
static void handle_thp_avl_msg(struct sqn_private *priv , struct sqn_lsp_packet *lsp) { struct sqn_sdio_card *card = priv->card; struct sk_buff *skb_reply = 0; enum sqn_thp_available_reply thp_rpl; unsigned long irq_flags = 0; sqn_pr_enter(); spin_lock_irqsave(&priv->drv_lock, irq_flags); /* if (card->is_card_sleeps) { */ if (priv->is_tx_queue_empty(priv)) { if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("TX queue empty, thp_rpl=FINISH\n"); } /* sqn_pr_dbg("card was asleep, thp_rpl=FINISH\n"); */ thp_rpl = LSP_THPA_FINISHED; card->is_card_sleeps = 1; gHostWakeupFWEvent = 0; /* } else if (priv->is_tx_queue_empty(priv)) { */ /* sqn_pr_dbg("card was not asleep and tx_queue is empty, thp_rpl=FINISHED\n"); */ /* thp_rpl = LSP_THPA_FINISHED; */ /* card->is_card_sleeps = 1; */ } else { /* sqn_pr_info("card was not asleep but tx_queue is no empty, thp_rpl=EXIT\n"); */ if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("TX queue not empty, thp_rpl=ACK\n"); } /* sqn_pr_dbg("card was not asleep, thp_rpl=ACK\n"); */ thp_rpl = LSP_THPA_ACK; card->is_card_sleeps = 0; } spin_unlock_irqrestore(&priv->drv_lock, irq_flags); skb_reply = construct_lsp_packet(THSP_THP_AVAILABLE_REPLY , ntohl(lsp->lsp_header.u.thp_avl.tid) , thp_rpl); if (0 != (skb_reply = sqn_sdio_prepare_skb_for_tx(skb_reply))) sqn_sdio_tx_skb(card, skb_reply, 0); wake_up_interruptible(&g_card_sleep_waitq); if (netif_queue_stopped(priv->dev)) netif_wake_queue(priv->dev); if (!card->is_card_sleeps && !gHostWakeupFWEvent) { gHostWakeupFWEvent = 1; // Dump next TX packet after LSP ThpAvailableReply(ACK); } sqn_pr_leave(); }
int init_thp(struct net_device* dev) { sqn_pr_enter(); #if THP_DEBUG printk(KERN_WARNING "init_thp +\n"); #endif if (0 == this_device) { if(init_procfs_handler()) { return -1; } if(init_thp_devfile()) return -1; /* Don't call init_thp_handler() here, it will be called from * probe() before interrupts are enabled, to ensure that we will * catch all THP packets as soon as they appear */ /* if (init_thp_handler(dev)) */ /* return -1; */ this_device = dev; sqn_pr_info("KTHP initialized\n"); } #if THP_DEBUG printk(KERN_WARNING "init_thp -\n"); #endif sqn_pr_leave(); return 0; }
int sqn_rx_process(struct net_device *dev, struct sk_buff *skb) { int rc = 0; struct sqn_private *priv = netdev_priv(dev); #if DRIVER_DEBUG printk(KERN_WARNING "sqn_rx_process \n"); #endif sqn_pr_enter(); dev->last_rx = jiffies; skb->protocol = eth_type_trans(skb, dev); skb->dev = dev; priv->stats.rx_packets++; priv->stats.rx_bytes += skb->len; #if SKB_DEBUG sqn_pr_info("%s: push skb [0x%p] to kernel, users %d\n", __func__, skb, atomic_read(&skb->users)); #endif netif_rx(skb); /* netif_receive_skb(skb); */ sqn_pr_leave(); return rc; }
static int sqn_handle_mac_addr_tag(struct sdio_func *func, u8 *data, u32 length) { int rv = 0; struct sqn_private *priv = ((struct sqn_sdio_card *)sdio_get_drvdata(func))->priv; sqn_pr_enter(); /* * This tag could contain one or two mac addresses in string * form, delimited by some symbol (space or something else). * Each mac address written as a string has constant length. * Thus we can determine the number of mac addresses by the * length of the tag: * * mac addr length in string form: XX:XX:XX:XX:XX:XX = 17 bytes * tag length: 17 bytes [ + 1 byte + 17 bytes ] */ #define MAC_ADDR_STRING_LEN 17 /* * If we have only one mac addr we should increment it by one * and use it. * If we have two mac addresses we should use a second one. */ if (MAC_ADDR_STRING_LEN <= length && length < 2 * MAC_ADDR_STRING_LEN + 1) { sqn_pr_dbg("single mac address\n"); /* we have only one mac addr */ get_mac_addr_from_str(data, length, priv->mac_addr); ++(priv->mac_addr[ETH_ALEN - 1]); } else if (2 * MAC_ADDR_STRING_LEN + 1 == length) { /* we have two macs */ sqn_pr_dbg("two mac addresses, using second\n"); get_mac_addr_from_str(data + MAC_ADDR_STRING_LEN + 1 , length - (MAC_ADDR_STRING_LEN + 1), priv->mac_addr); } else { /* incorrect data length */ sqn_pr_err("can't get mac address from bootloader" " - incorrect mac address length\n"); rv = -1; goto out; } sqn_pr_info("setting MAC address from bootloader: " "%02x:%02x:%02x:%02x:%02x:%02x\n", priv->mac_addr[0] , priv->mac_addr[1], priv->mac_addr[2], priv->mac_addr[3] , priv->mac_addr[4], priv->mac_addr[5]); out: sqn_pr_leave(); return rv; }
int sqn_notify_host_sleep(struct sdio_func *func) { int rv = 0; unsigned long irq_flags = 0; u32 timeout = 0; struct sqn_sdio_card *sqn_card = sdio_get_drvdata(func); sqn_pr_enter(); sqn_card->waiting_pm_notification = 1; sqn_pr_info("notify card about host goes to sleep...\n"); sqn_set_host_power_mode(func, LSP_HPM_ASLEEP); timeout = 50; sqn_pr_info("wait for completion (timeout %u msec)...\n", timeout); rv = wait_event_interruptible_timeout(sqn_card->pm_waitq , sqn_card->pm_complete, msecs_to_jiffies(timeout)); if (-ERESTARTSYS == rv) { sqn_pr_warn("got a signal from kernel %d\n", rv); } else if (0 == rv) { /* a timeout elapsed */ sqn_pr_warn("timeout elapsed - still no ack from card" ", assume that card in sleep mode now\n"); sqn_card->is_card_sleeps = 1; } else { /* we got an ack from card */ sqn_pr_info("card in sleep mode now\n"); sqn_card->is_card_sleeps = 1; rv = 0; } sqn_card->pm_complete = 0; sqn_card->waiting_pm_notification = 0; sqn_pr_leave(); return rv; }
static ssize_t sqn_dfs_fetch_rx_read(struct file *file, char __user *ubuf, size_t count, loff_t *ppos) { char buf[1024]; int max = sizeof(buf) - 1; int i = 0; sqn_pr_info("fetching RX packets\n"); sdio_claim_host(_g_sqn_sdio_card->func); sqn_sdio_it_lsb(_g_sqn_sdio_card->func); sdio_release_host(_g_sqn_sdio_card->func); i += scnprintf(buf + i, max - i, "RX packets fetched\n"); return simple_read_from_buffer(ubuf, count, ppos, buf, i); }
int thp_handler(struct sk_buff *skb, struct net_device *pDev, struct packet_type *pPt) #endif { struct sk_buff *skb_thp = 0; struct ethhdr *eth = 0; sqn_pr_enter(); /* We need only ETH_P_802_2 protocol packets with THP mac address */ eth = skb2ethhdr(skb); if(ntohs(skb->protocol) != ETH_P_802_2 || !is_thp_packet(eth->h_dest)) { //for DDTM, drop all NOT THP packets if(drop_packet) { sqn_pr_dbg("HTC CODE: drop packet for DDTM\n"); skb->pkt_type = PACKET_OTHERHOST; } goto not_thp_out; } skb_thp = skb_clone(skb, GFP_ATOMIC); /* Bugz 22554: strip CRC at the end of packet */ skb_trim(skb_thp, skb_thp->len - 4); #if THP_TRACE sqn_pr_info("%s: RX packet, len = %d\n", __func__, skb_thp->len); #endif sqn_pr_dbg("RX THP packet, length %d\n", skb_thp->len); skb_queue_tail(&to_sqntool_queue, skb_thp); if(skb_queue_len(&to_sqntool_queue) == 256){ skb_thp = skb_dequeue(&to_sqntool_queue); kfree_skb(skb_thp); } wake_up_interruptible(&to_sqntool_wait); //Wake up wait queue thp_out: dev_kfree_skb_any(skb); sqn_pr_leave(); return NET_RX_DROP; not_thp_out: dev_kfree_skb_any(skb); sqn_pr_leave(); return NET_RX_SUCCESS; }
void cleanup_thp(void) { sqn_pr_enter(); #if THP_DEBUG printk(KERN_WARNING "cleanup_thp +\n"); #endif if (this_device) { cleanup_procfs_handler(); cleanup_thp_handler(); cleanup_thp_devfile(); this_device = 0; sqn_pr_info("KTHP cleaned up\n"); } #if THP_DEBUG printk(KERN_WARNING "cleanup_thp -\n"); #endif sqn_pr_leave(); }
int sqn_start_card(struct sqn_private *priv) { struct net_device *dev = priv->dev; sqn_pr_enter(); if (register_netdev(dev)) { sqn_pr_err("cannot register ethX device\n"); return -1; } sqn_pr_dbg("starting TX thread...\n"); /* TODO: move waitq initializatio to add_card() */ init_waitqueue_head(&priv->tx_waitq); init_waitqueue_head(&priv->rx_waitq); if (sqn_start_tx_thread(priv)) goto err_init_adapter; sqn_pr_info("%s: Sequans WiMAX adapter\n", dev->name); #if IGNORE_CARRIER_STATE netif_carrier_on(priv->dev); #else /* In release version this should be uncommented */ /* netif_carrier_off(priv->dev); */ #endif done: sqn_pr_leave(); return 0; err_init_adapter: /* TODO: Free allocated resources */ sqn_pr_err("error while init adapter\n"); free_netdev(dev); priv = NULL; goto done; }
static ssize_t thp_read(struct file *filp, char *buf, size_t count, loff_t*ppos) { DECLARE_WAITQUEUE(wait, current); struct sk_buff_head *head = &to_sqntool_queue; struct sk_buff *curr = NULL; ssize_t retval; const struct sqn_thp_header *th = 0; sqn_pr_enter(); #if THP_DEBUG printk(KERN_WARNING "thp_read +\n"); #endif add_wait_queue(&to_sqntool_wait, &wait); retval = -ERESTARTSYS; if(0 == this_device) { printk(KERN_WARNING "thp_read() device removed\n"); retval = -EINVAL; goto out; } while(1) { if(!skb_queue_empty(head)) break; if(signal_pending(current) || 0 == this_device) { printk(KERN_WARNING "thp_read() interrupted by signal\n"); retval = -EINTR; goto out; } set_current_state(TASK_INTERRUPTIBLE); schedule(); } curr = skb_dequeue(head); if (count < curr->len) { printk(KERN_WARNING "%s: userspace buffer is too small (%u bytes)" " to hold THP packet (%u bytes)\n" , __func__, count, curr->len); retval = -EINVAL; goto free_skb; } else { count = curr->len; } if(copy_to_user(buf, curr->data, count)) { printk(KERN_ERR "error copying data to user space\n"); retval = -EFAULT; goto free_skb; } if (mmc_wimax_get_thp_log()) { sqn_pr_info("%s: [to_user]: len = %d\n", __func__, count); th = (struct sqn_thp_header *) curr->data; sqn_pr_info("%s: PKTLen: %4u | TVer: 0x0%x | Flags: 0x0%x | Len: %4u" " | SeqNum: %5u | AckNum: %5u | TLen: %5u\n", __func__ , count , th->transport_version , th->flags , be16_to_cpu(th->length) , be16_to_cpu(th->seq_number) , be16_to_cpu(th->ack_number) , be32_to_cpu(th->total_length)); sqn_pr_dbg_dump("THP RX:", curr->data, count); } #if SKB_DEBUG sqn_pr_info("%s: free skb [0x%p], users %d\n", __func__, curr, atomic_read(&curr->users)); #endif retval = (ssize_t)count; free_skb: dev_kfree_skb_any(curr); out: set_current_state(TASK_RUNNING); remove_wait_queue(&to_sqntool_wait, &wait); #if THP_DEBUG printk(KERN_WARNING "thp_read -\n"); #endif sqn_pr_leave(); return retval; }
int sqn_wakeup_fw(struct sdio_func *func) { int rv = 0; int ver = 0; int counter = 0; int retry_cnt = 3; u32 wakeup_delay = 0; unsigned long timeout = msecs_to_jiffies(800); unsigned long irq_flags = 0; struct sqn_private *priv = ((struct sqn_sdio_card *)sdio_get_drvdata(func))->priv; struct sqn_sdio_card *card = priv->card; u8 need_to_unlock_wakelock = 0; sqn_pr_enter(); sqn_pr_info("waking up the card...\n"); if (!wake_lock_active(&card->wakelock_tx)) { if (mmc_wimax_get_sdio_wakelock_log()) { printk(KERN_INFO "[WIMAX] lock wl_tx2,"); PRINTRTC; } wake_lock(&card->wakelock_tx); need_to_unlock_wakelock = 1; } retry: if (priv->removed) goto out; sdio_claim_host(func); #define SDIO_CCCR_CCCR_SDIO_VERSION_VALUE 0x11 wakeup_delay = 2; counter = 5; do { sqn_pr_dbg("CMD52 #%d, delay %d msec\n", counter, wakeup_delay); ver = sdio_readb(func, SDIO_CCCR_CCCR_SDIO_VERSION, &rv); // To avoid FW sutck in PLLOFF, SDIO isn't able to wake up it. mdelay(wakeup_delay); --counter; } while((rv || ver != SDIO_CCCR_CCCR_SDIO_VERSION_VALUE) && counter > 0); if (rv) { sqn_pr_err("error when reading SDIO_VERSION\n"); if (mmc_wimax_get_wimax_FW_freeze_WK_TX()) { sqn_pr_info("[ste]set is_card_sleeps 0 to avoid TX polling\n"); card->is_card_sleeps = 0; } sdio_release_host(func); goto out; } else sqn_pr_dbg("SDIO_VERSION has been read successfully\n"); sqn_pr_dbg("send wake-up signal to card\n"); sdio_writeb(func, 1, SQN_SOC_SIGS_LSBS, &rv); if (rv) sqn_pr_err("error when writing to SQN_SOC_SIGS_LSBS: %d\n", rv); sdio_release_host(func); sqn_pr_info("wait for completion (timeout %d msec)...\n" , jiffies_to_msecs(timeout)); rv = wait_event_interruptible_timeout(g_card_sleep_waitq , 0 == card->is_card_sleeps || priv->removed, timeout); if (priv->removed) goto out; if (-ERESTARTSYS == rv) { sqn_pr_warn("got a signal from kernel %d\n", rv); } else if (0 == rv) { rv = -1; sqn_pr_err("can't wake up the card - timeout elapsed\n"); if (retry_cnt-- > 0 && card->is_card_sleeps) { sqn_pr_info("retry wake up\n"); goto retry; } sqn_pr_info("giving up to wake up the card\n"); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 0; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); } else { rv = 0; sqn_pr_info("card is waked up successfully\n"); } out: if (need_to_unlock_wakelock && wake_lock_active(&card->wakelock_tx)) { if (mmc_wimax_get_sdio_wakelock_log()) { printk(KERN_INFO "[WIMAX] release wake_lock_tx in %s,", __func__); PRINTRTC; } wake_unlock(&card->wakelock_tx); /* TX */ } sqn_pr_leave(); return rv; }
int sqn_handle_lsp_packet(struct sqn_private *priv , struct sk_buff *skb) { struct sqn_sdio_card *card = priv->card; unsigned long irq_flags = 0; struct sqn_lsp_packet *lsp_response = (struct sqn_lsp_packet*) ((u8*)skb->data + sizeof(struct sqn_eth_header)); sqn_pr_enter(); if (!sqn_is_rx_lsp_packet(skb)) { sqn_pr_dbg("not LSP packet\n"); sqn_pr_leave(); return 0; } sqn_pr_dbg("LSP packet\n"); switch (ntohl(lsp_response->lsp_header.id)) { case THSP_GET_MEDIA_CONNECTION_STATE: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: THSP_GET_MEDIA_CONNECTION_STATE state=%xh\n" , ntohl(lsp_response->lsp_header.u.media_con_state)); } sqn_pr_warn("THSP_GET_MEDIA_CONNECTION_STATE not implemented\n"); break; case THSP_MEDIA_CONNECTION_STATE_CHANGE: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: THSP_MEDIA_CONNECTION_STATE_CHANGE state=%xh\n" , ntohl(lsp_response->lsp_header.u.media_con_state)); } if (THSP_MEDIA_CONNECTION_ATTACHED == ntohl(lsp_response->lsp_header.u.media_con_state)) { #if IGNORE_CARRIER_STATE /* netif_carrier_on(priv->dev); */ sqn_pr_info("WiMAX carrier PRESENT [ignored]\n"); #else netif_carrier_on(priv->dev); sqn_pr_info("WiMAX carrier PRESENT\n"); #endif } else { #if IGNORE_CARRIER_STATE /* netif_carrier_off(priv->dev); */ sqn_pr_info("WiMAX carrier LOST [ignored]\n"); #else netif_carrier_off(priv->dev); sqn_pr_info("WiMAX carrier LOST\n"); #endif } break; case THSP_SET_HOST_POWER_MODE_ACK: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: THSP_SET_HOST_POWER_MODE_ACK tid=0x%x\n" , ntohl(lsp_response->lsp_header.u.host_power.tid)); } free_last_request(); spin_lock_irqsave(&priv->drv_lock, irq_flags); card->is_card_sleeps = 1; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); signal_pm_request_completion(priv); break; case THSP_SET_FW_POWER_MODE_ACK: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: THSP_SET_FW_POWER_MODE_ACK tid=0x%x\n" , ntohl(lsp_response->lsp_header.u.fw_power.tid)); } sqn_pr_info("THSP_SET_FW_POWER_MODE_ACK not implemented\n"); break; case THSP_SQN_STATE_CHANGE: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: THSP_SQN_STATE_CHANGE tid=0x%x, state=%xh\n" , ntohl(lsp_response->lsp_header.u.fw_state.tid) , ntohl(lsp_response->lsp_header.u.fw_state.state)); } handle_sqn_state_change_msg(priv, lsp_response); break; case THSP_THP_AVAILABLE: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: THSP_THP_AVAILABLE tid=0x%x, reply=%xh\n" , ntohl(lsp_response->lsp_header.u.thp_avl.tid) , ntohl(lsp_response->lsp_header.u.thp_avl.reply)); } handle_thp_avl_msg(priv, lsp_response); break; default: if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("RX LSP: UNKNOWN=0x%x\n" , ntohl(lsp_response->lsp_header.id)); } } dev_kfree_skb_any(skb); sqn_pr_leave(); return 1; }
static struct sk_buff *lsp_to_skb(struct sqn_lsp_packet *lsp_packet) /* RX */ { struct sqn_eth_header eth_header = { .len = htons(sizeof(struct sqn_lsp_packet)) }; struct sk_buff *skb = __dev_alloc_skb(sizeof(eth_header) + sizeof(struct sqn_lsp_packet) , GFP_ATOMIC | GFP_DMA); sqn_pr_enter(); if (0 == skb) goto out; skb_reserve(skb, 2); memcpy(eth_header.dst_addr, g_lsp_device_mac, sizeof(g_lsp_device_mac)); memcpy(eth_header.src_addr, g_lsp_host_mac, sizeof(g_lsp_host_mac)); memcpy(skb->data, ð_header, sizeof(eth_header)); skb_put(skb, sizeof(eth_header)); memcpy(skb->data + skb->len, lsp_packet, sizeof(struct sqn_lsp_packet)); skb_put(skb, sizeof(struct sqn_lsp_packet)); sqn_pr_leave(); out: return skb; } static struct sk_buff* construct_lsp_packet(u32 id, u32 param1, u32 param2) { struct sqn_lsp_packet lsp_packet = { .thp_header = { .transport_version = 1 , .flags = 1 , .seq_number = 0 , .ack_number = 0 } , .lsp_header = { .id = htonl(id) } }; struct sk_buff *skb = 0; sqn_pr_enter(); switch (id) { case THSP_GET_MEDIA_CONNECTION_STATE: /* no parameters are needed */ if (mmc_wimax_get_sdio_lsp_log()) sqn_pr_info("TX LSP: THSP_GET_MEDIA_CONNECTION_STATE\n"); lsp_packet.thp_header.length = htons(sizeof(struct sqn_lsp_header) - 4); lsp_packet.thp_header.total_length = htonl(sizeof(struct sqn_lsp_header) - 4); break; case THSP_SET_POWER_MODE: /* deprecated */ if (mmc_wimax_get_sdio_lsp_log()) sqn_pr_info("TX LSP: THSP_SET_POWER_MODE (deprecated)\n"); break; case THSP_SET_HOST_POWER_MODE: lsp_packet.thp_header.length = htons(sizeof(struct sqn_lsp_header)); lsp_packet.thp_header.total_length = htonl(sizeof(struct sqn_lsp_header)); lsp_packet.lsp_header.u.host_power.tid = htonl(get_next_tid()); lsp_packet.lsp_header.u.host_power.mode = htonl(param1); if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("TX LSP: THSP_SET_HOST_POWER_MODE, tid: 0x%x, mode: %d\n" , ntohl(lsp_packet.lsp_header.u.host_power.tid) , param1); } break; case THSP_SET_FW_POWER_MODE: lsp_packet.thp_header.length = htons(sizeof(struct sqn_lsp_header)); lsp_packet.thp_header.total_length = htonl(sizeof(struct sqn_lsp_header)); lsp_packet.lsp_header.u.fw_power.tid = htonl(get_next_tid()); lsp_packet.lsp_header.u.fw_power.mode = htonl(param1); if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("TX LSP: THSP_SET_FW_POWER_MODE, tid: 0x%x, mode: %d\n" , htonl(lsp_packet.lsp_header.u.fw_power.tid) , param1); } break; case THSP_SQN_STATE_CHANGE_REPLY: lsp_packet.thp_header.length = htons(sizeof(struct sqn_lsp_header) - 4); lsp_packet.thp_header.total_length = htonl(sizeof(struct sqn_lsp_header) - 4); lsp_packet.lsp_header.u.fw_state.tid = htonl(param1); if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("TX LSP: THSP_SQN_STATE_CHANGE_REPLY, tid: %xh\n" , param1); } break; case THSP_THP_AVAILABLE_REPLY: lsp_packet.thp_header.length = htons(sizeof(struct sqn_lsp_header)); lsp_packet.thp_header.total_length = htonl(sizeof(struct sqn_lsp_header)); lsp_packet.lsp_header.u.thp_avl.tid = htonl(param1); lsp_packet.lsp_header.u.thp_avl.reply = htonl(param2); if (mmc_wimax_get_sdio_lsp_log()) { sqn_pr_info("TX LSP: THSP_THP_AVAILABLE_REPLY, tid: 0x%x, reply: %d\n" , param1, param2); } break; default: if (mmc_wimax_get_sdio_lsp_log()) sqn_pr_info("TX LSP: UNKNOWN\n"); } skb = lsp_to_skb(&lsp_packet); sqn_pr_leave(); return skb; }
static ssize_t thp_write(struct file *file, const char *buf, size_t count, loff_t *ppos) { ssize_t retval = -ENOMEM; struct sk_buff *skb; struct ethhdr ethh; int size = count + ETH_HLEN; const struct sqn_thp_header *th = 0; sqn_pr_enter(); #if THP_DEBUG printk(KERN_WARNING "thp_write +\n"); #endif if(0 == this_device) return -ENODEV; skb = __dev_alloc_skb(size, GFP_ATOMIC | GFP_DMA); if(skb == NULL) return retval; #if SKB_DEBUG sqn_pr_info("%s: [0x%p] alloc skb, users %d\n", __func__, skb, atomic_read(&skb->users)); #endif memcpy(ethh.h_dest, ss_macaddr, ETH_ALEN); memcpy(ethh.h_source, host_macaddr, ETH_ALEN); ethh.h_proto = htons(count); memcpy(skb->data, ðh, sizeof(struct ethhdr)); skb_put(skb, sizeof(struct ethhdr)); if(copy_from_user(skb->tail, buf, count)) { dev_kfree_skb_any(skb); return -EFAULT; } skb_put(skb, count); if (mmc_wimax_get_thp_log()) { sqn_pr_info("%s: [from_user]: len = %d\n", __func__, count); th = (struct sqn_thp_header *) buf; sqn_pr_info("%s: PKTLen: %4u | TVer: 0x0%x | Flags: 0x0%x | Len: %4u" " | SeqNum: %5u | AckNum: %5u | TLen: %5u\n", __func__ , count , th->transport_version , th->flags , be16_to_cpu(th->length) , be16_to_cpu(th->seq_number) , be16_to_cpu(th->ack_number) , be32_to_cpu(th->total_length)); sqn_pr_dbg_dump("THP TX:", skb->data, count); } #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 29) this_device->hard_start_xmit(skb, this_device); #else this_device->netdev_ops->ndo_start_xmit(skb, this_device); #endif retval = count; #if THP_DEBUG printk(KERN_WARNING "thp_write -\n"); #endif sqn_pr_leave(); return retval; }
int sqn_hard_start_xmit(struct sk_buff *skb, struct net_device *dev) { unsigned long irq_flags = 0; struct ethhdr *eth; struct sqn_private *priv = netdev_priv(dev); #if DRIVER_DEBUG printk(KERN_WARNING "sqn_hard_start_xmit \n"); #endif sqn_pr_enter(); sqn_pr_dbg("skb->len = %d\n", skb->len); #if SKB_DEBUG sqn_pr_info("%s: got skb [0x%p] from kernel, users %d\n", __func__, skb, atomic_read(&skb->users)); #endif spin_lock_irqsave(&priv->drv_lock, irq_flags); //HTC code: for DDTM if(drop_packet){ eth = (struct ethhdr*) skb->data; if(memcmp(eth->h_dest, ss_macaddr, ETH_ALEN) != 0){ sqn_pr_dbg("HTC drop_packet enabled: not THP, drop it\n"); #if DRIVER_DEBUG printk(KERN_WARNING "sqn_hard_start_xmit: network packet\n"); #endif priv->stats.tx_dropped++; priv->stats.tx_errors++; dev_kfree_skb_any(skb); goto out; }else{ sqn_pr_dbg("HTC drop_packet enabled: THP, let it live\n"); #if DRIVER_DEBUG printk(KERN_WARNING "sqn_hard_start_xmit: thp packet\n"); #endif } } if (priv->removed) goto out; if (skb->len < 1 || (skb->len > SQN_MAX_PDU_LEN)) { sqn_pr_dbg("skb length %d not in range (1, %d)\n", skb->len, SQN_MAX_PDU_LEN); /* * We'll never manage to send this one; * drop it and return 'OK' */ priv->stats.tx_dropped++; priv->stats.tx_errors++; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); goto out; } /* netif_stop_queue(priv->dev); */ priv->add_skb_to_tx_queue(priv, skb, 1); priv->stats.tx_packets++; priv->stats.tx_bytes += skb->len; dev->trans_start = jiffies; spin_unlock_irqrestore(&priv->drv_lock, irq_flags); wake_up_interruptible(&priv->tx_waitq); out: sqn_pr_leave(); return NETDEV_TX_OK; }