/** 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;
}
