caif_hsi.c revision b42f7b5cfda6f7dac298da2d9a8855f6364e35d9
140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copyright (C) ST-Ericsson AB 2010 340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com 440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Author: Daniel Martensson / daniel.martensson@stericsson.com 540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Dmitry.Tarnyagin / dmitry.tarnyagin@stericsson.com 640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * License terms: GNU General Public License (GPL) version 2. 740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com#define pr_fmt(fmt) KBUILD_MODNAME fmt 1039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 1140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/init.h> 1240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/module.h> 1340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/device.h> 1440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/platform_device.h> 1540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/netdevice.h> 1640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/string.h> 1740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/list.h> 1840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/interrupt.h> 1940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/delay.h> 2040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/sched.h> 2140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/if_arp.h> 2240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/timer.h> 235bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson#include <linux/rtnetlink.h> 24ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin#include <linux/pkt_sched.h> 2540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <net/caif/caif_layer.h> 2640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <net/caif/caif_hsi.h> 2740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 2840d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_LICENSE("GPL"); 2940d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>"); 3040d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_DESCRIPTION("CAIF HSI driver"); 3140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* Returns the number of padding bytes for alignment. */ 3340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\ 3440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (((pow)-((x)&((pow)-1))))) 3540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyaginstatic int inactivity_timeout = 1000; 3728bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyaginmodule_param(inactivity_timeout, int, S_IRUGO | S_IWUSR); 3828bd2049428202cb3bc982536ed3de3c69ae120aDmitry TarnyaginMODULE_PARM_DESC(inactivity_timeout, "Inactivity timeout on HSI, ms."); 3928bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin 40ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic int aggregation_timeout = 1; 41ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginmodule_param(aggregation_timeout, int, S_IRUGO | S_IWUSR); 42ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry TarnyaginMODULE_PARM_DESC(aggregation_timeout, "Aggregation timeout on HSI, ms."); 43ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 4440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 4540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * HSI padding options. 4640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Warning: must be a base of 2 (& operation used) and can not be zero ! 4740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 4840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_head_align = 4; 4940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_head_align, int, S_IRUGO); 5040d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_head_align, "HSI head alignment."); 5140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_tail_align = 4; 5340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_tail_align, int, S_IRUGO); 5440d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_tail_align, "HSI tail alignment."); 5540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 5740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * HSI link layer flowcontrol thresholds. 5840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Warning: A high threshold value migth increase throughput but it will at 5940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * the same time prevent channel prioritization and increase the risk of 6040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * flooding the modem. The high threshold should be above the low. 6140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 6240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_high_threshold = 100; 6340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_high_threshold, int, S_IRUGO); 6440d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_high_threshold, "HSI high threshold (FLOW OFF)."); 6540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 6640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_low_threshold = 50; 6740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_low_threshold, int, S_IRUGO); 6840d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_low_threshold, "HSI high threshold (FLOW ON)."); 6940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define ON 1 7140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define OFF 0 7240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 7440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Threshold values for the HSI packet queue. Flowcontrol will be asserted 7540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * when the number of packets exceeds HIGH_WATER_MARK. It will not be 7640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * de-asserted before the number of packets drops below LOW_WATER_MARK. 7740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 7840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define LOW_WATER_MARK hsi_low_threshold 7940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define HIGH_WATER_MARK hsi_high_threshold 8040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic LIST_HEAD(cfhsi_list); 8240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic spinlock_t cfhsi_list_lock; 8340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_inactivity_tout(unsigned long arg) 8540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 8640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = (struct cfhsi *)arg; 8740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 8940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 9040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule power down work queue. */ 9240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 9340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_down_work); 9440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 9540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 96ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_update_aggregation_stats(struct cfhsi *cfhsi, 97ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin const struct sk_buff *skb, 98ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int direction) 99ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 100ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct caif_payload_info *info; 101ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int hpad, tpad, len; 102ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 103ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin info = (struct caif_payload_info *)&skb->cb; 104ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 105ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 106ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin len = skb->len + hpad + tpad; 107ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 108ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (direction > 0) 109ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_len += len; 110ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin else if (direction < 0) 111ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_len -= len; 112ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 113ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 114ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic bool cfhsi_can_send_aggregate(struct cfhsi *cfhsi) 115ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 116ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i; 117ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 118a5c96b518c373fba35a8dc2e4e5fead24aeefb1cKim Lilliestierna XX if (cfhsi->aggregation_timeout == 0) 119ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return true; 120ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 121ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_BEBK; ++i) { 122ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi->qhead[i].qlen) 123ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return true; 124ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 125ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 126ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* TODO: Use aggregation_len instead */ 127ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi->qhead[CFHSI_PRIO_BEBK].qlen >= CFHSI_MAX_PKTS) 128ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return true; 129ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 130ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return false; 131ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 132ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 133ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic struct sk_buff *cfhsi_dequeue(struct cfhsi *cfhsi) 134ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 135ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct sk_buff *skb; 136ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i; 137ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 138ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_LAST; ++i) { 139ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = skb_dequeue(&cfhsi->qhead[i]); 140ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (skb) 141ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 142ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 143ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 144ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return skb; 145ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 146ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 147ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic int cfhsi_tx_queue_len(struct cfhsi *cfhsi) 148ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 149ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i, len = 0; 150ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_LAST; ++i) 151ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin len += skb_queue_len(&cfhsi->qhead[i]); 152ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return len; 153ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 154ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 15540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_abort_tx(struct cfhsi *cfhsi) 15640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 15740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 15840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 15940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin for (;;) { 16040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 161ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = cfhsi_dequeue(cfhsi); 16240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 16340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 16440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 16540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_errors++; 16640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_dropped++; 167ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, -1); 16840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 16940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree_skb(skb); 17040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 17140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 17240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 173ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->inactivity_timer, 17428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 17540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 17640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 17740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_flush_fifo(struct cfhsi *cfhsi) 17940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 18040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin char buffer[32]; /* Any reasonable value */ 18140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin size_t fifo_occupancy; 18240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int ret; 18340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 18540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 18640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 18840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 18940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &fifo_occupancy); 19040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 19190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 19240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't get FIFO occupancy: %d.\n", 19340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 19440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 19540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!fifo_occupancy) 19640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* No more data, exitting normally */ 19740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 19840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 19940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin fifo_occupancy = min(sizeof(buffer), fifo_occupancy); 20040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 20140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_rx(buffer, fifo_occupancy, 20240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev); 20340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 20440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 20590de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 20640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't read data: %d.\n", 20740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 20840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 20940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 21040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = 5 * HZ; 212687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->flush_fifo_wait, 21340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret); 21440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 21690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 21740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't wait for flush complete: %d.\n", 21840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 21940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 22040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 22140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = -ETIMEDOUT; 22290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 22340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: timeout waiting for flush complete.\n", 22440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 22540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 22640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 22740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (1); 22840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return ret; 23040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 23140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 23340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 23440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 23540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int pld_len = 0; 23640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 23740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 23840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 239ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = cfhsi_dequeue(cfhsi); 24040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 24140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 24240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24394230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com /* Clear offset. */ 24494230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com desc->offset = 0; 24594230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com 24640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can embed a CAIF frame. */ 24740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (skb->len < CFHSI_MAX_EMB_FRM_SZ) { 24840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 249b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int hpad; 250b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int tpad; 25140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 25340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 25440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 25640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 25740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if frame still fits with added alignment. */ 25940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) { 26040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pemb = desc->emb_frm; 26140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->offset = CFHSI_DESC_SHORT_SZ; 26240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pemb = (u8)(hpad - 1); 26340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pemb += hpad; 26440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 26540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 266ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 26740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 26840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 269ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, -1); 270ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 27140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in embedded CAIF frame. */ 27340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pemb, skb->len); 274ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 275ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Consume the SKB */ 27640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 27740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 27840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 27994230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com } 28040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create payload CAIF frames. */ 28240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 28340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 28440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 285b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int hpad; 286b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int tpad; 28740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 289ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = cfhsi_dequeue(cfhsi); 29040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 29240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 29340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 29540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 29640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 29840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 29940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill in CAIF frame length in descriptor. */ 30140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = hpad + skb->len + tpad; 30240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill head padding information. */ 30440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pfrm = (u8)(hpad - 1); 30540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += hpad; 30640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 308ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 30940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 31040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 311ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, -1); 312ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 31340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 31440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in CAIF frame. */ 31540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pfrm, skb->len); 31640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 31740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update payload length. */ 31840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pld_len += desc->cffrm_len[nfrms]; 31940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 32040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update frame pointer. */ 32140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += skb->len + tpad; 322ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 323ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Consume the SKB */ 32440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 32540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 32640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 32740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update number of frames. */ 32840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 32940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 33040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unused length fields should be zero-filled (according to SPEC). */ 33240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 33340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = 0x0000; 33440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 33540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 33640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can piggy-back another descriptor. */ 338ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi_can_send_aggregate(cfhsi)) 33940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header |= CFHSI_PIGGY_DESC; 34040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 34140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header &= ~CFHSI_PIGGY_DESC; 34240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 34340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return CFHSI_DESC_SZ + pld_len; 34440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 34540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 346ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_start_tx(struct cfhsi *cfhsi) 34740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 348ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf; 349ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int len, res; 35040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 35190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", __func__); 35240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 35340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 35440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 35540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 35640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 35740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 358ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 359ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (!len) { 360ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 361ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (unlikely(cfhsi_tx_queue_len(cfhsi))) { 362fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 363ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin res = -EAGAIN; 364ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin continue; 365fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } 366ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 367ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Start inactivity timer. */ 368ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->inactivity_timer, 369ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 370ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 371ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 372ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 37340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 37440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 37540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 376ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (WARN_ON(res < 0)) 37790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: TX error %d.\n", 37840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 37940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (res < 0); 380ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 381ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 382ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_tx_done(struct cfhsi *cfhsi) 383ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 38490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", __func__); 385ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 386ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 387ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return; 388ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 389ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* 390ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin * Send flow on if flow off has been previously signalled 391ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin * and number of packets is below low water mark. 392ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin */ 393ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 394ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi->flow_off_sent && 395ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_tx_queue_len(cfhsi) <= cfhsi->q_low_mark && 396ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->cfdev.flowctrl) { 397ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 398ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->flow_off_sent = 0; 399ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, ON); 400ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 401ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 402ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi_can_send_aggregate(cfhsi)) { 403ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 404ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_start_tx(cfhsi); 405ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } else { 406ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->aggregation_timer, 407ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin jiffies + cfhsi->aggregation_timeout); 408ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 409ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 410fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin 411fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin return; 41240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 41340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 41440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_tx_done_cb(struct cfhsi_drv *drv) 41540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 41640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 41740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 41840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 41990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 42040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 42140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 42240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 42340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 424687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_tx_done(cfhsi); 42540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 42640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4275bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 42840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 42940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int xfer_sz = 0; 43040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 43140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 43240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 43340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 43440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((desc->header & ~CFHSI_PIGGY_DESC) || 43540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) { 43690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n", 43740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 4385bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 43940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 44040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 44140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for embedded CAIF frame. */ 44240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->offset) { 44340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 44440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 445687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int len = 0; 44640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = ((u8 *)desc) + desc->offset; 44740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 44840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove offset padding. */ 44940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *pfrm + 1; 45040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 45140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 45240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pfrm; 45340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pfrm+1)) << 8) & 0xFF00; 45440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 45540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4565bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frame. */ 4575bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 45890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid length.\n", 4595bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 4605bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 4615bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 46240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 46340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 464687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 465687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 46690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Out of memory !\n", 467687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 468687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 46940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 47040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 47140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 47340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pfrm, len); 47440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 47640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 47740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 47840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 48040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * We are called from a arch specific platform device. 48140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Unfortunately we don't know what context we're 48240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * running in. 48340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 48440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 48540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 48640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 48740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 48840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 48940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 49040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 49140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 49240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 49340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 49440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate transfer length. */ 49540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 49640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 49740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += *plen; 49840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 49940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 50040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 50140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 50240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for piggy-backed descriptor. */ 50340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->header & CFHSI_PIGGY_DESC) 50440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += CFHSI_DESC_SZ; 50540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5065bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) { 50790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, 50840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Invalid payload len: %d, ignored.\n", 50940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, xfer_sz); 5105bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 51140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 51240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return xfer_sz; 51340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 51440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 515332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.comstatic int cfhsi_rx_desc_len(struct cfhsi_desc *desc) 516332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com{ 517332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com int xfer_sz = 0; 518332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com int nfrms = 0; 519332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com u16 *plen; 520332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 521332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if ((desc->header & ~CFHSI_PIGGY_DESC) || 522332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) { 523332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 524332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com pr_err("Invalid descriptor. %x %x\n", desc->header, 525332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com desc->offset); 526332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com return -EPROTO; 527332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 528332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 529332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Calculate transfer length. */ 530332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com plen = desc->cffrm_len; 531332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com while (nfrms < CFHSI_MAX_PKTS && *plen) { 532332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com xfer_sz += *plen; 533332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com plen++; 534332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com nfrms++; 535332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 536332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 537332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (xfer_sz % 4) { 538332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com pr_err("Invalid payload len: %d, ignored.\n", xfer_sz); 539332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com return -EPROTO; 540332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 541332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com return xfer_sz; 542332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com} 543332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 5445bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 54540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 54640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int rx_sz = 0; 54740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 54840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 54940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 55040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 55140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check header and offset. */ 55240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) || 55340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) { 55490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n", 55540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 5565bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 55740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 55840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 55940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set frame pointer to start of payload. */ 56040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 56140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 562687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 563687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Skip already processed frames. */ 564687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (nfrms < cfhsi->rx_state.nfrms) { 565687b13e98addc99644002703944ec89e94287cb6Daniel Martensson pfrm += *plen; 566687b13e98addc99644002703944ec89e94287cb6Daniel Martensson rx_sz += *plen; 567687b13e98addc99644002703944ec89e94287cb6Daniel Martensson plen++; 568687b13e98addc99644002703944ec89e94287cb6Daniel Martensson nfrms++; 569687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 570687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 571687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Parse payload. */ 57240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 57340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 57440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 57540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pcffrm = NULL; 576b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int len; 57740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 57840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* CAIF frame starts after head padding. */ 57940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pcffrm = pfrm + *pfrm + 1; 58040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 58140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 58240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pcffrm; 58340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pcffrm + 1)) << 8) & 0xFF00; 58440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 58540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5865bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frames. */ 5875bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 58890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid length.\n", 5895bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 5905bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 5915bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 5925bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 59340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 594687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 595687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 59690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Out of memory !\n", 597687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 598687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.nfrms = nfrms; 599687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 60040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 60140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 60240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 60340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 60440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pcffrm, len); 60540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 60640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 60740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 60840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 60940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 61040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 61140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * We're called from a platform device, 61240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and don't know the context we're running in. 61340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 61440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 61540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 61640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 61740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 61840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 61940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 62040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 62140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 62240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 62340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *plen; 62440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_sz += *plen; 62540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 62640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 62740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 62840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 62940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return rx_sz; 63040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 63140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 632687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_done(struct cfhsi *cfhsi) 63340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 63440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 635332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com int desc_pld_len = 0, rx_len, rx_state; 63640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = NULL; 637332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com u8 *rx_ptr, *rx_buf; 638332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com struct cfhsi_desc *piggy_desc = NULL; 63940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc = (struct cfhsi_desc *)cfhsi->rx_buf; 64140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s\n", __func__); 64340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 64540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 64640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update inactivity timer if pending. */ 64873033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 649ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer_pending(&cfhsi->inactivity_timer, 65028bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 65173033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 65240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 653687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) { 654332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com desc_pld_len = cfhsi_rx_desc_len(desc); 655332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 656332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len < 0) 6575bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 658332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 659332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_buf = cfhsi->rx_buf; 660332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = desc_pld_len; 661332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len > 0 && (desc->header & CFHSI_PIGGY_DESC)) 662332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len += CFHSI_DESC_SZ; 663332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len == 0) 664332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_buf = cfhsi->rx_flip_buf; 66540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 666332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_buf = cfhsi->rx_flip_buf; 66740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 668332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = CFHSI_DESC_SZ; 669332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi->rx_state.pld_len > 0 && 670332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com (desc->header & CFHSI_PIGGY_DESC)) { 67140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin piggy_desc = (struct cfhsi_desc *) 67340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ + 674332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.pld_len); 675332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 676687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.piggy_desc = true; 67740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 678332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract payload len from piggy-backed descriptor. */ 679332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com desc_pld_len = cfhsi_rx_desc_len(piggy_desc); 680332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len < 0) 681332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 682332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 6834e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX if (desc_pld_len > 0) { 684332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = desc_pld_len; 6854e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX if (piggy_desc->header & CFHSI_PIGGY_DESC) 6864e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX rx_len += CFHSI_DESC_SZ; 6874e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX } 68840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 68940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 69040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copy needed information from the piggy-backed 69140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * descriptor to the descriptor in the start. 69240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 693332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com memcpy(rx_buf, (u8 *)piggy_desc, 69440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin CFHSI_DESC_SHORT_SZ); 695332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Mark no embedded frame here */ 696332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com piggy_desc->offset = 0; 6975bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 698687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 699687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 70040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc_pld_len) { 701332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_state = CFHSI_RX_STATE_PAYLOAD; 702332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_ptr = rx_buf + CFHSI_DESC_SZ; 70340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 704332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_state = CFHSI_RX_STATE_DESC; 705332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_ptr = rx_buf; 706332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = CFHSI_DESC_SZ; 70740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 70840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 709332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Initiate next read */ 71040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) { 71140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 71290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", 713332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com __func__); 714332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 715332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com res = cfhsi->dev->cfhsi_rx(rx_ptr, rx_len, 71640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev); 71740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 71890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: RX error %d.\n", 71940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 72040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_errors++; 72140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_dropped++; 72240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 72340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 724687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 725332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) { 726332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract payload from descriptor */ 727332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi_rx_desc(desc, cfhsi) < 0) 728332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 729332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } else { 730332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract payload */ 731332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi_rx_pld(desc, cfhsi) < 0) 732332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 733332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (piggy_desc) { 734332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract any payload in piggyback descriptor. */ 735332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi_rx_desc(piggy_desc, cfhsi) < 0) 736332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 737332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 738687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 739332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 740332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Update state info */ 741332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state)); 742332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.state = rx_state; 743332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_ptr = rx_ptr; 744332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_len = rx_len; 745332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.pld_len = desc_pld_len; 746332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.piggy_desc = desc->header & CFHSI_PIGGY_DESC; 747332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 748332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (rx_buf != cfhsi->rx_buf) 749332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com swap(cfhsi->rx_buf, cfhsi->rx_flip_buf); 7505bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return; 7515bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 7525bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonout_of_sync: 75390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Out of sync.\n", __func__); 7545bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE, 7555bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi->rx_buf, CFHSI_DESC_SZ); 7565bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson schedule_work(&cfhsi->out_of_sync_work); 757687b13e98addc99644002703944ec89e94287cb6Daniel Martensson} 758687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 759687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_slowpath(unsigned long arg) 760687b13e98addc99644002703944ec89e94287cb6Daniel Martensson{ 761687b13e98addc99644002703944ec89e94287cb6Daniel Martensson struct cfhsi *cfhsi = (struct cfhsi *)arg; 762687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 76390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 764687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 765687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 766687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 76740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 76840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 76940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_rx_done_cb(struct cfhsi_drv *drv) 77040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 77140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 77240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 77490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 77540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 77640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 77840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 77940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 78040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits)) 78140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->flush_fifo_wait); 78240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 783687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 78440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 78540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 78640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up(struct work_struct *work) 78740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 78840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 78940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 79040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 79140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 79240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_up_work); 79440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 79640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 79740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) { 79940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* It happenes when wakeup is requested by 80040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * both ends at the same time. */ 80140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 8025ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 80340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 80440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 80540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 80640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate wake line. */ 80740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_up(cfhsi->dev); 80840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 80990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Start waiting.\n", 81040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 81140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 81240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 813687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 814687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait, 815687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_UP_ACK, 81640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &cfhsi->bits), ret); 81740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(ret < 0)) { 81840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 81990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n", 82040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 8215ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 82240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 82340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 82440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 82540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 8265ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson bool ca_wake = false; 8275ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson size_t fifo_occupancy = 0; 8285ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 82940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wakeup timeout */ 83090de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Timeout.\n", 83140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 8325ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8335ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check FIFO to check if modem has sent something. */ 8345ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 8355ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &fifo_occupancy)); 8365ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 83790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Bytes in FIFO: %u.\n", 8385ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__, (unsigned) fifo_occupancy); 8395ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8405ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check if we misssed the interrupt. */ 8415ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev, 8425ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &ca_wake)); 8435ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8445ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson if (ca_wake) { 84590de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n", 8465ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__); 8475ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8485ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */ 8495ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 8505ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8515ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Continue execution. */ 8525ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson goto wake_ack; 8535ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson } 8545ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 85540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 85640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 85740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 85840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 8595ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martenssonwake_ack: 86090de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Woken.\n", 86140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 86240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up bit. */ 86440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_AWAKE, &cfhsi->bits); 86540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 86640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Resume read operation. */ 86890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", __func__); 869687b13e98addc99644002703944ec89e94287cb6Daniel Martensson res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->dev); 870687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 871687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (WARN_ON(res < 0)) 87290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: RX err %d.\n", __func__, res); 87340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up acknowledment. */ 87540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 87640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 87840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 879ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Resume transmit if queues are not empty. */ 880ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (!cfhsi_tx_queue_len(cfhsi)) { 88190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Peer wake, start timer.\n", 88240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 88340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Start inactivity timer. */ 884ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->inactivity_timer, 88528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 88640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 88740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 88840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 88940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89090de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Host wake.\n", 89140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 89240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 89440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 89640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi); 89740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (likely(len > 0)) { 89940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 90040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 90140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 90290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: TX error %d.\n", 90340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 90440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 90540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 90640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 90790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, 90840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Failed to create HSI frame: %d.\n", 90940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, len); 91040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 91140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 91240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 91340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down(struct work_struct *work) 91440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 91540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 91640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 917687b13e98addc99644002703944ec89e94287cb6Daniel Martensson size_t fifo_occupancy = 0; 918687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int retry = CFHSI_WAKE_TOUT; 91940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_down_work); 92190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", __func__); 92240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 92440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 92540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate wake line. */ 92740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 92840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 930687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 93140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait, 932687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_DOWN_ACK, 933687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &cfhsi->bits), ret); 93440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 93540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 93690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n", 93740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 93840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 93940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 9405ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson bool ca_wake = true; 9415ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 94240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Timeout */ 94390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Timeout.\n", __func__); 9445ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 9455ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check if we misssed the interrupt. */ 9465ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev, 9475ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &ca_wake)); 9485ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson if (!ca_wake) 94990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n", 9505ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__); 95140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 95240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 953687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Check FIFO occupancy. */ 954687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (retry) { 955687b13e98addc99644002703944ec89e94287cb6Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 956687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &fifo_occupancy)); 957687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 958687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!fifo_occupancy) 959687b13e98addc99644002703944ec89e94287cb6Daniel Martensson break; 960687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 961687b13e98addc99644002703944ec89e94287cb6Daniel Martensson set_current_state(TASK_INTERRUPTIBLE); 962687b13e98addc99644002703944ec89e94287cb6Daniel Martensson schedule_timeout(1); 963687b13e98addc99644002703944ec89e94287cb6Daniel Martensson retry--; 964687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 965687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 966687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!retry) 96790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: FIFO Timeout.\n", __func__); 968687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 969687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Clear AWAKE condition. */ 97040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 97140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 972687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Cancel pending RX requests. */ 973687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev); 97440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 97540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 97640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9775bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic void cfhsi_out_of_sync(struct work_struct *work) 9785bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson{ 9795bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson struct cfhsi *cfhsi = NULL; 9805bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 9815bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi = container_of(work, struct cfhsi, out_of_sync_work); 9825bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 9835bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_lock(); 9845bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_close(cfhsi->ndev); 9855bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_unlock(); 9865bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson} 9875bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 98840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up_cb(struct cfhsi_drv *drv) 98940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 99040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 99140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 99390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 99440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 99540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 99740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_up_wait); 99840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 100040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 100140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 100240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the peer initiates. */ 100340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 100440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 100540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 100640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 100740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down_cb(struct cfhsi_drv *drv) 100840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 100940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 101040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 101140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 101290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 101340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 101440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 101540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initiating low power is only permitted by the host (us). */ 101640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 101740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_down_wait); 101840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 101940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1020ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_aggregation_tout(unsigned long arg) 1021ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 1022ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct cfhsi *cfhsi = (struct cfhsi *)arg; 1023ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 102490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 1025ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin __func__); 1026ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 1027ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_start_tx(cfhsi); 1028ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 1029ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 103040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev) 103140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 103240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 103340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int start_xfer = 0; 103440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int timer_active; 1035ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int prio; 103640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 103740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!dev) 103840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 103940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 104040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(dev); 104140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1042ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin switch (skb->priority) { 1043ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_BESTEFFORT: 1044ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_FILLER: 1045ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_BULK: 1046ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_BEBK; 1047ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1048ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_INTERACTIVE_BULK: 1049ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_VI; 1050ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1051ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_INTERACTIVE: 1052ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_VO; 1053ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1054ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_CONTROL: 1055ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin default: 1056ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_CTL; 1057ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1058ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 1059ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 106040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 106140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1062ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Update aggregation statistics */ 1063ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, 1); 1064ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 1065ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Queue the SKB */ 1066ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb_queue_tail(&cfhsi->qhead[prio], skb); 106740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 106840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check; xmit should not be called after unregister_netdev */ 106940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) { 107040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 107140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 107240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 107340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 107440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 107540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Send flow off if number of packets is above high water mark. */ 107640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->flow_off_sent && 1077ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_tx_queue_len(cfhsi) > cfhsi->q_high_mark && 107840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl) { 107940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 1; 108040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF); 108140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 108240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) { 108440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_XFER; 108540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin start_xfer = 1; 108640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 108740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108873033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin if (!start_xfer) { 1089ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Send aggregate if it is possible */ 1090ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin bool aggregate_ready = 1091ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_can_send_aggregate(cfhsi) && 1092ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin del_timer(&cfhsi->aggregation_timer) > 0; 109373033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 1094ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (aggregate_ready) 1095ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_start_tx(cfhsi); 109640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 109773033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin } 109840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 109940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Delete inactivity timer if started. */ 1100ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin timer_active = del_timer_sync(&cfhsi->inactivity_timer); 110140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110273033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 110373033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin 110440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (timer_active) { 110540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf; 110640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 110740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 110840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 111040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 1111f84ea779c25dabc90956f1c329e5e5c501ea96ccRoar Førde WARN_ON(!len); 111240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 111340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 111440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 111540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 111690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: TX error %d.\n", 111740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 111840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 111940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 112040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 112140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the we initiate. */ 112240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 112340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 112440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 112540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 112640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 112740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 112840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 112939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.comstatic const struct net_device_ops cfhsi_ops; 113040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_setup(struct net_device *dev) 113240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 1133ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i; 113440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = netdev_priv(dev); 113540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->features = 0; 113640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->netdev_ops = &cfhsi_ops; 113740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->type = ARPHRD_CAIF; 113840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->flags = IFF_POINTOPOINT | IFF_NOARP; 113934efc283a56adaef5756ac93065f46608674ea7dSjur Brændeland dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ; 114040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->tx_queue_len = 0; 114140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->destructor = free_netdev; 1142ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_LAST; ++i) 1143ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb_queue_head_init(&cfhsi->qhead[i]); 114440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW; 114540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_frag = false; 114640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_stx = false; 114740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_fcs = false; 114840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = dev; 114940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 115040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginint cfhsi_probe(struct platform_device *pdev) 115240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 115340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 115440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct net_device *ndev; 115539abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 115640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 115740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup); 11597ac2ed0ceeafa130f85aa947b271b571c68b9e75Joe Perches if (!ndev) 116040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -ENODEV; 116140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 116240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(ndev); 116340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = ndev; 116440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->pdev = pdev; 116540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 116639abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com /* Assign the HSI device. */ 116739abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com cfhsi->dev = pdev->dev.platform_data; 116839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 116939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com /* Assign the driver to this HSI device. */ 117039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com cfhsi->dev->drv = &cfhsi->drv; 117139abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 117239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com /* Register network device. */ 117339abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com res = register_netdev(ndev); 117439abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com if (res) { 117539abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com dev_err(&ndev->dev, "%s: Registration error: %d.\n", 117639abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com __func__, res); 117739abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com free_netdev(ndev); 117839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com } 117939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com /* Add CAIF HSI device to list. */ 118039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com spin_lock(&cfhsi_list_lock); 118139abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com list_add_tail(&cfhsi->list, &cfhsi_list); 118239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com spin_unlock(&cfhsi_list_lock); 118339abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 118439abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com return res; 118539abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com} 118639abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 118739abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.comstatic int cfhsi_open(struct net_device *ndev) 118839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com{ 118939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com struct cfhsi *cfhsi = netdev_priv(ndev); 119039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com int res; 119139abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 119239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com clear_bit(CFHSI_SHUTDOWN, &cfhsi->bits); 119339abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 119440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize state vaiables. */ 119540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 1196687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_DESC; 119740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 119840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set flow info */ 119940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 0; 120040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->q_low_mark = LOW_WATER_MARK; 120140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->q_high_mark = HIGH_WATER_MARK; 120240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 120340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 120440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 120540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a TX buffer with the size of a HSI packet descriptors 120640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and the necessary room for CAIF payload frames. 120740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 120840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL); 120940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->tx_buf) { 121040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 121140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_tx; 121240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 121340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 121540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a RX buffer with the size of two HSI packet descriptors and 121640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * the necessary room for CAIF payload frames. 121740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 121840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL); 121940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->rx_buf) { 122040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 122140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_rx; 122240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 122340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1224332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_flip_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL); 1225332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (!cfhsi->rx_flip_buf) { 1226332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com res = -ENODEV; 1227332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto err_alloc_rx_flip; 1228332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 1229332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 123028bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Pre-calculate inactivity timeout. */ 123128bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin if (inactivity_timeout != -1) { 123228bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = 123328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin inactivity_timeout * HZ / 1000; 123428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin if (!cfhsi->inactivity_timeout) 123528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = 1; 123628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin else if (cfhsi->inactivity_timeout > NEXT_TIMER_MAX_DELTA) 123728bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA; 123828bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin } else { 123928bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA; 124028bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin } 124128bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin 1242ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Initialize aggregation timeout */ 1243ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_timeout = aggregation_timeout; 1244ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 124528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Initialize recieve vaiables. */ 124640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf; 124740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = CFHSI_DESC_SZ; 124840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin locks. */ 125040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi->lock); 125140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 125240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up the driver. */ 125340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb; 125440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb; 125594230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb; 125694230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb; 125740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 125840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize the work queues. */ 125940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up); 126040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down); 12615bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync); 126240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 126340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear all bit fields. */ 126440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 126540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 126640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 126740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 126840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 126940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create work thread. */ 127039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com cfhsi->wq = create_singlethread_workqueue(cfhsi->pdev->name); 127140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->wq) { 127290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Failed to create work queue.\n", 127340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 127440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 127540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_create_wq; 127640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 127740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 127840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize wait queues. */ 127940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_up_wait); 128040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_down_wait); 128140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->flush_fifo_wait); 128240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 128340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Setup the inactivity timer. */ 1284ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin init_timer(&cfhsi->inactivity_timer); 1285ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->inactivity_timer.data = (unsigned long)cfhsi; 1286ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->inactivity_timer.function = cfhsi_inactivity_tout; 1287687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Setup the slowpath RX timer. */ 1288687b13e98addc99644002703944ec89e94287cb6Daniel Martensson init_timer(&cfhsi->rx_slowpath_timer); 1289687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi; 1290687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath; 1291ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Setup the aggregation timer. */ 1292ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin init_timer(&cfhsi->aggregation_timer); 1293ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_timer.data = (unsigned long)cfhsi; 1294ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_timer.function = cfhsi_aggregation_tout; 129540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 129640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate HSI interface. */ 129740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_up(cfhsi->dev); 129840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 129990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, 130040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't activate HSI interface: %d.\n", 130140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 130240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_activate; 130340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 130440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 130540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush FIFO */ 130640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi_flush_fifo(cfhsi); 130740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 130890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Can't flush FIFO: %d.\n", 130940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 131040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_net_reg; 131140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 131240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 131340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 131440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_net_reg: 131540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_down(cfhsi->dev); 131640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_activate: 131740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 131840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_create_wq: 1319332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com kfree(cfhsi->rx_flip_buf); 1320332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com err_alloc_rx_flip: 132140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->rx_buf); 132240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_rx: 132340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->tx_buf); 132440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_tx: 132540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 132640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 132740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 132839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.comstatic int cfhsi_close(struct net_device *ndev) 132940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 133039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com struct cfhsi *cfhsi = netdev_priv(ndev); 13315f614e6b7005685b3e34da0742f79389c858c7e1sjur.brandeland@stericsson.com u8 *tx_buf, *rx_buf, *flip_buf; 133240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 133340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* going to shutdown driver */ 133440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_SHUTDOWN, &cfhsi->bits); 133540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 133640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush workqueue */ 133740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin flush_workqueue(cfhsi->wq); 133840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1339687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Delete timers if pending */ 1340ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin del_timer_sync(&cfhsi->inactivity_timer); 1341687b13e98addc99644002703944ec89e94287cb6Daniel Martensson del_timer_sync(&cfhsi->rx_slowpath_timer); 1342ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin del_timer_sync(&cfhsi->aggregation_timer); 134340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 134440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Cancel pending RX request (if any) */ 134540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev); 134640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1347ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson /* Destroy workqueue */ 134840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 134940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 135040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Store bufferes: will be freed later. */ 135140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tx_buf = cfhsi->tx_buf; 135240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_buf = cfhsi->rx_buf; 13535f614e6b7005685b3e34da0742f79389c858c7e1sjur.brandeland@stericsson.com flip_buf = cfhsi->rx_flip_buf; 135440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush transmit queues. */ 135540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 135640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 135740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate interface */ 135840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_down(cfhsi->dev); 135940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 136040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Free buffers. */ 136140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(tx_buf); 136240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(rx_buf); 13635f614e6b7005685b3e34da0742f79389c858c7e1sjur.brandeland@stericsson.com kfree(flip_buf); 136439abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com return 0; 136540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 136640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 136739abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.comstatic const struct net_device_ops cfhsi_ops = { 136839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com .ndo_open = cfhsi_open, 136939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com .ndo_stop = cfhsi_close, 137039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com .ndo_start_xmit = cfhsi_xmit 137139abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com}; 137239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 137340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginint cfhsi_remove(struct platform_device *pdev) 137440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 137540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *list_node; 137640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *n; 137740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 137840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_dev *dev; 137940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 138040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev = (struct cfhsi_dev *)pdev->dev.platform_data; 138140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 138240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_for_each_safe(list_node, n, &cfhsi_list) { 138340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = list_entry(list_node, struct cfhsi, list); 138440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Find the corresponding device. */ 138540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->dev == dev) { 138640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove from list. */ 138740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_del(list_node); 138840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 138940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 139040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 139140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 139240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 139340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -ENODEV; 139440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 139540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 139640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstruct platform_driver cfhsi_plat_drv = { 139740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .probe = cfhsi_probe, 139840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .remove = cfhsi_remove, 139940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .driver = { 140040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .name = "cfhsi", 140140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .owner = THIS_MODULE, 140240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin }, 140340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin}; 140440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 140540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void __exit cfhsi_exit_module(void) 140640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 140740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *list_node; 140840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *n; 140940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 141040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 141140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 141240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_for_each_safe(list_node, n, &cfhsi_list) { 141340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = list_entry(list_node, struct cfhsi, list); 141440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 141540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove from list. */ 141640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_del(list_node); 141740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 141840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 141939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com unregister_netdevice(cfhsi->ndev); 142040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 142140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 142240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 142340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 142440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 142540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unregister platform driver. */ 142640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin platform_driver_unregister(&cfhsi_plat_drv); 142740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 142840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 142940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int __init cfhsi_init_module(void) 143040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 143140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int result; 143240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 143340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin lock. */ 143440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi_list_lock); 143540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 143640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Register platform driver. */ 143740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin result = platform_driver_register(&cfhsi_plat_drv); 143840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (result) { 143940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin printk(KERN_ERR "Could not register platform HSI driver: %d.\n", 144040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin result); 144140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_dev_register; 144240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 144340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 144440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_dev_register: 144540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return result; 144640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 144740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 144840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_init(cfhsi_init_module); 144940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_exit(cfhsi_exit_module); 1450