140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copyright (C) ST-Ericsson AB 2010 3c2cd0a560c129d5f01be1df62bfc412d3aff6953sjur.brandeland@stericsson.com * Author: Daniel Martensson 4c002090cfaad91859ae25bb7ee24dac8ec1f3188sjur.brandeland@stericsson.com * Dmitry.Tarnyagin / dmitry.tarnyagin@lockless.no 540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * License terms: GNU General Public License (GPL) version 2. 640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com#define pr_fmt(fmt) KBUILD_MODNAME fmt 939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 1040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/init.h> 1140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/module.h> 1240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/device.h> 1340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/netdevice.h> 1440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/string.h> 1540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/list.h> 1640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/interrupt.h> 1740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/delay.h> 1840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/sched.h> 1940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/if_arp.h> 2040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/timer.h> 21c41254006377842013922fb1e407391f24c59522Sjur Brændeland#include <net/rtnetlink.h> 22ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin#include <linux/pkt_sched.h> 2340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <net/caif/caif_layer.h> 2440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <net/caif/caif_hsi.h> 2540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 2640d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_LICENSE("GPL"); 27c2cd0a560c129d5f01be1df62bfc412d3aff6953sjur.brandeland@stericsson.comMODULE_AUTHOR("Daniel Martensson"); 2840d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_DESCRIPTION("CAIF HSI driver"); 2940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* Returns the number of padding bytes for alignment. */ 3140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\ 3240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (((pow)-((x)&((pow)-1))))) 3340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændelandstatic const struct cfhsi_config hsi_default_config = { 3528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin 3691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland /* Inactivity timeout on HSI, ms */ 3791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland .inactivity_timeout = HZ, 38ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 3991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland /* Aggregation timeout (ms) of zero means no aggregation is done*/ 4091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland .aggregation_timeout = 1, 4140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland /* 4391fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * HSI link layer flow-control thresholds. 4491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * Threshold values for the HSI packet queue. Flow-control will be 4591fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * asserted when the number of packets exceeds q_high_mark. It will 4691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * not be de-asserted before the number of packets drops below 4791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * q_low_mark. 4891fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * Warning: A high threshold value might increase throughput but it 4991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * will at the same time prevent channel prioritization and increase 5091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * the risk of flooding the modem. The high threshold should be above 5191fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * the low. 5291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland */ 5391fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland .q_high_mark = 100, 5491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland .q_low_mark = 50, 5540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland /* 5791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * HSI padding options. 5891fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * Warning: must be a base of 2 (& operation used) and can not be zero ! 5991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland */ 6091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland .head_align = 4, 6191fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland .tail_align = 4, 6291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland}; 6340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 6440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define ON 1 6540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define OFF 0 6640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 6740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic LIST_HEAD(cfhsi_list); 6840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 6940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_inactivity_tout(unsigned long arg) 7040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 7140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = (struct cfhsi *)arg; 7240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 7440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 7540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule power down work queue. */ 7740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 7840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_down_work); 7940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 8040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 81ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_update_aggregation_stats(struct cfhsi *cfhsi, 82ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin const struct sk_buff *skb, 83ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int direction) 84ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 85ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct caif_payload_info *info; 86ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int hpad, tpad, len; 87ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 88ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin info = (struct caif_payload_info *)&skb->cb; 8991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align); 9091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align); 91ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin len = skb->len + hpad + tpad; 92ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 93ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (direction > 0) 94ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_len += len; 95ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin else if (direction < 0) 96ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_len -= len; 97ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 98ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 99ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic bool cfhsi_can_send_aggregate(struct cfhsi *cfhsi) 100ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 101ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i; 102ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 10391fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland if (cfhsi->cfg.aggregation_timeout == 0) 104ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return true; 105ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 106ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_BEBK; ++i) { 107ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi->qhead[i].qlen) 108ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return true; 109ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 110ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 111ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* TODO: Use aggregation_len instead */ 112ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi->qhead[CFHSI_PRIO_BEBK].qlen >= CFHSI_MAX_PKTS) 113ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return true; 114ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 115ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return false; 116ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 117ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 118ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic struct sk_buff *cfhsi_dequeue(struct cfhsi *cfhsi) 119ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 120ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct sk_buff *skb; 121ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i; 122ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 123ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_LAST; ++i) { 124ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = skb_dequeue(&cfhsi->qhead[i]); 125ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (skb) 126ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 127ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 128ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 129ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return skb; 130ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 131ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 132ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic int cfhsi_tx_queue_len(struct cfhsi *cfhsi) 133ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 134ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i, len = 0; 135ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_LAST; ++i) 136ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin len += skb_queue_len(&cfhsi->qhead[i]); 137ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return len; 138ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 139ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 14040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_abort_tx(struct cfhsi *cfhsi) 14140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 14240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 14340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 14440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin for (;;) { 14540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 146ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = cfhsi_dequeue(cfhsi); 14740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 14840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 14940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 15040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_errors++; 15140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_dropped++; 152ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, -1); 15340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 15440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree_skb(skb); 15540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 15640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 15740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 158ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->inactivity_timer, 15991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland jiffies + cfhsi->cfg.inactivity_timeout); 16040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 16140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 16240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 16340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_flush_fifo(struct cfhsi *cfhsi) 16440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 16540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin char buffer[32]; /* Any reasonable value */ 16640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin size_t fifo_occupancy; 16740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int ret; 16840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 16990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 17040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 17140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 1731c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland ret = cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops, 17440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &fifo_occupancy); 17540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 17690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 17740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't get FIFO occupancy: %d.\n", 17840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 17940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 18040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!fifo_occupancy) 18140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* No more data, exitting normally */ 18240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 18340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin fifo_occupancy = min(sizeof(buffer), fifo_occupancy); 18540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 1861c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland ret = cfhsi->ops->cfhsi_rx(buffer, fifo_occupancy, 1871c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops); 18840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 18940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 19090de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 19140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't read data: %d.\n", 19240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 19340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 19440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 19540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 19640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = 5 * HZ; 197687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->flush_fifo_wait, 19840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret); 19940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 20040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 20190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 20240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't wait for flush complete: %d.\n", 20340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 20440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 20540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 20640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = -ETIMEDOUT; 20790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_warn(cfhsi->ndev, 20840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: timeout waiting for flush complete.\n", 20940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 21040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 21140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 21240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (1); 21340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return ret; 21540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 21640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 21840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 21940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 22040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int pld_len = 0; 22140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 22240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 22340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 224ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = cfhsi_dequeue(cfhsi); 22540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 22640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 22740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22894230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com /* Clear offset. */ 22994230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com desc->offset = 0; 23094230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com 23140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can embed a CAIF frame. */ 23240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (skb->len < CFHSI_MAX_EMB_FRM_SZ) { 23340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 234b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int hpad; 235b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int tpad; 23640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 23840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 23940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align); 24191fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align); 24240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if frame still fits with added alignment. */ 24440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) { 24540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pemb = desc->emb_frm; 24640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->offset = CFHSI_DESC_SHORT_SZ; 24740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pemb = (u8)(hpad - 1); 24840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pemb += hpad; 24940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 251ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 25240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 25340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 254ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, -1); 255ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 25640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in embedded CAIF frame. */ 25840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pemb, skb->len); 259ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 260ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Consume the SKB */ 26140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 26240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 26340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 26494230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com } 26540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 26640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create payload CAIF frames. */ 26740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 26840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 26940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 270b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int hpad; 271b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int tpad; 27240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 274ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb = cfhsi_dequeue(cfhsi); 27540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 27740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 27840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 28040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 28140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland hpad = 1 + PAD_POW2((info->hdr_len + 1), cfhsi->cfg.head_align); 28391fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland tpad = PAD_POW2((skb->len + hpad), cfhsi->cfg.tail_align); 28440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill in CAIF frame length in descriptor. */ 28640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = hpad + skb->len + tpad; 28740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill head padding information. */ 28940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pfrm = (u8)(hpad - 1); 29040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += hpad; 29140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 293ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 29440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 29540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 296ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, -1); 297ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 29840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in CAIF frame. */ 30040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pfrm, skb->len); 30140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update payload length. */ 30340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pld_len += desc->cffrm_len[nfrms]; 30440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update frame pointer. */ 30640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += skb->len + tpad; 307ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 308ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Consume the SKB */ 30940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 31040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 31140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 31240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update number of frames. */ 31340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 31440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 31540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 31640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unused length fields should be zero-filled (according to SPEC). */ 31740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 31840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = 0x0000; 31940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 32040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 32140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 32240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can piggy-back another descriptor. */ 323ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi_can_send_aggregate(cfhsi)) 32440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header |= CFHSI_PIGGY_DESC; 32540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 32640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header &= ~CFHSI_PIGGY_DESC; 32740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 32840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return CFHSI_DESC_SZ + pld_len; 32940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 33040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 331ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_start_tx(struct cfhsi *cfhsi) 33240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 333ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf; 334ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int len, res; 33540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", __func__); 33740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 33940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 34040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 34140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 34240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 343ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 344ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (!len) { 345ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 346ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (unlikely(cfhsi_tx_queue_len(cfhsi))) { 347fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 348ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin res = -EAGAIN; 349ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin continue; 350fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } 351ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 352ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Start inactivity timer. */ 353ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->inactivity_timer, 35491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland jiffies + cfhsi->cfg.inactivity_timeout); 355ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 356ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 357ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 35840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 35940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 3601c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops); 361ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (WARN_ON(res < 0)) 36290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: TX error %d.\n", 36340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 36440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (res < 0); 365ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 366ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 367ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_tx_done(struct cfhsi *cfhsi) 368ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 36990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", __func__); 370ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 371ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 372ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin return; 373ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 374ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* 375ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin * Send flow on if flow off has been previously signalled 376ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin * and number of packets is below low water mark. 377ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin */ 378ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 379ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi->flow_off_sent && 38091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi_tx_queue_len(cfhsi) <= cfhsi->cfg.q_low_mark && 381ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->cfdev.flowctrl) { 382ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 383ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->flow_off_sent = 0; 384ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, ON); 385ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 386ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 387ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (cfhsi_can_send_aggregate(cfhsi)) { 388ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 389ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_start_tx(cfhsi); 390ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } else { 391ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->aggregation_timer, 39291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland jiffies + cfhsi->cfg.aggregation_timeout); 393ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 394ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 395fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin 396fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin return; 39740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 39840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3991c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændelandstatic void cfhsi_tx_done_cb(struct cfhsi_cb_ops *cb_ops) 40040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 40140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 40240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4031c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi = container_of(cb_ops, struct cfhsi, cb_ops); 40490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 40540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 40640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 40740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 40840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 409687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_tx_done(cfhsi); 41040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 41140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4125bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 41340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 41440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int xfer_sz = 0; 41540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 41640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 41740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 41840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 41940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((desc->header & ~CFHSI_PIGGY_DESC) || 42040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) { 42190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n", 42240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 4235bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 42440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 42540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 42640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for embedded CAIF frame. */ 42740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->offset) { 42840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 42940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 430687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int len = 0; 43140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = ((u8 *)desc) + desc->offset; 43240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 43340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove offset padding. */ 43440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *pfrm + 1; 43540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 43640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 43740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pfrm; 43840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pfrm+1)) << 8) & 0xFF00; 43940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 44040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frame. */ 4425bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 44390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid length.\n", 4445bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 4455bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 4465bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 44740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 44840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 449687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 450687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 45190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Out of memory !\n", 452687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 453687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 45440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 45540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 45640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 45740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 45840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pfrm, len); 45940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 46040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 46140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 46240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 46340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 46440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 4651c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland * We are in a callback handler and 4661c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland * unfortunately we don't know what context we're 46740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * running in. 46840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 46940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 47040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 47140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 47240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 47340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 47540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 47640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 47740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 47840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate transfer length. */ 48040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 48140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 48240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += *plen; 48340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 48440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 48540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 48640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 48740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for piggy-backed descriptor. */ 48840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->header & CFHSI_PIGGY_DESC) 48940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += CFHSI_DESC_SZ; 49040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4915bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) { 49290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, 49340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Invalid payload len: %d, ignored.\n", 49440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, xfer_sz); 4955bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 49640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 49740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return xfer_sz; 49840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 49940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 500332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.comstatic int cfhsi_rx_desc_len(struct cfhsi_desc *desc) 501332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com{ 502332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com int xfer_sz = 0; 503332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com int nfrms = 0; 504332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com u16 *plen; 505332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 506332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if ((desc->header & ~CFHSI_PIGGY_DESC) || 507332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) { 508332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 509332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com pr_err("Invalid descriptor. %x %x\n", desc->header, 510332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com desc->offset); 511332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com return -EPROTO; 512332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 513332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 514332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Calculate transfer length. */ 515332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com plen = desc->cffrm_len; 516332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com while (nfrms < CFHSI_MAX_PKTS && *plen) { 517332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com xfer_sz += *plen; 518332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com plen++; 519332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com nfrms++; 520332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 521332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 522332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (xfer_sz % 4) { 523332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com pr_err("Invalid payload len: %d, ignored.\n", xfer_sz); 524332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com return -EPROTO; 525332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 526332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com return xfer_sz; 527332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com} 528332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 5295bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 53040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 53140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int rx_sz = 0; 53240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 53340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 53440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 53540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 53640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check header and offset. */ 53740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) || 53840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) { 53990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid descriptor.\n", 54040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 5415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 54240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 54340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 54440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set frame pointer to start of payload. */ 54540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 54640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 547687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 548687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Skip already processed frames. */ 549687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (nfrms < cfhsi->rx_state.nfrms) { 550687b13e98addc99644002703944ec89e94287cb6Daniel Martensson pfrm += *plen; 551687b13e98addc99644002703944ec89e94287cb6Daniel Martensson rx_sz += *plen; 552687b13e98addc99644002703944ec89e94287cb6Daniel Martensson plen++; 553687b13e98addc99644002703944ec89e94287cb6Daniel Martensson nfrms++; 554687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 555687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 556687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Parse payload. */ 55740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 55840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 55940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 56040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pcffrm = NULL; 561b42f7b5cfda6f7dac298da2d9a8855f6364e35d9Sjur Brændeland int len; 56240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 56340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* CAIF frame starts after head padding. */ 56440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pcffrm = pfrm + *pfrm + 1; 56540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 56640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 56740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pcffrm; 56840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pcffrm + 1)) << 8) & 0xFF00; 56940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 57040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5715bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frames. */ 5725bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 57390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Invalid length.\n", 5745bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 5755bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 5765bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 5775bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 57840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 579687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 580687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 58190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Out of memory !\n", 582687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 583687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.nfrms = nfrms; 584687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 58540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 58640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 58740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 58840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 58940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pcffrm, len); 59040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 59140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 59240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 59340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 59440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 59540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 5961c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland * We're called in callback from HSI 59740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and don't know the context we're running in. 59840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 59940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 60040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 60140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 60240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 60340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 60440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 60540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 60640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 60740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 60840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *plen; 60940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_sz += *plen; 61040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 61140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 61240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 61340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 61440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return rx_sz; 61540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 61640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 617687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_done(struct cfhsi *cfhsi) 61840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 61940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 620332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com int desc_pld_len = 0, rx_len, rx_state; 62140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = NULL; 622332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com u8 *rx_ptr, *rx_buf; 623332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com struct cfhsi_desc *piggy_desc = NULL; 62440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 62540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc = (struct cfhsi_desc *)cfhsi->rx_buf; 62640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 62790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s\n", __func__); 62840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 62940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 63040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 63140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 63240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update inactivity timer if pending. */ 63373033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 634ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer_pending(&cfhsi->inactivity_timer, 63591fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland jiffies + cfhsi->cfg.inactivity_timeout); 63673033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 63740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 638687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) { 639332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com desc_pld_len = cfhsi_rx_desc_len(desc); 640332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 641332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len < 0) 6425bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 643332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 644332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_buf = cfhsi->rx_buf; 645332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = desc_pld_len; 646332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len > 0 && (desc->header & CFHSI_PIGGY_DESC)) 647332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len += CFHSI_DESC_SZ; 648332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len == 0) 649332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_buf = cfhsi->rx_flip_buf; 65040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 651332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_buf = cfhsi->rx_flip_buf; 65240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 653332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = CFHSI_DESC_SZ; 654332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi->rx_state.pld_len > 0 && 655332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com (desc->header & CFHSI_PIGGY_DESC)) { 65640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 65740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin piggy_desc = (struct cfhsi_desc *) 65840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ + 659332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.pld_len); 660332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 661687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.piggy_desc = true; 66240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 663332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract payload len from piggy-backed descriptor. */ 664332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com desc_pld_len = cfhsi_rx_desc_len(piggy_desc); 665332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (desc_pld_len < 0) 666332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 667332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 6684e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX if (desc_pld_len > 0) { 669332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = desc_pld_len; 6704e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX if (piggy_desc->header & CFHSI_PIGGY_DESC) 6714e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX rx_len += CFHSI_DESC_SZ; 6724e7bb59d49fb00d4cf13484386d0400783f2c826Kim Lilliestierna XX } 67340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 67540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copy needed information from the piggy-backed 67640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * descriptor to the descriptor in the start. 67740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 678332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com memcpy(rx_buf, (u8 *)piggy_desc, 67940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin CFHSI_DESC_SHORT_SZ); 6805bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 681687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 682687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 68340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc_pld_len) { 684332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_state = CFHSI_RX_STATE_PAYLOAD; 685332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_ptr = rx_buf + CFHSI_DESC_SZ; 68640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 687332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_state = CFHSI_RX_STATE_DESC; 688332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_ptr = rx_buf; 689332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com rx_len = CFHSI_DESC_SZ; 69040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 69140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 692332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Initiate next read */ 69340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) { 69440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 69590de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", 696332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com __func__); 697332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 6981c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland res = cfhsi->ops->cfhsi_rx(rx_ptr, rx_len, 6991c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops); 70040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 70190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: RX error %d.\n", 70240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 70340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_errors++; 70440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_dropped++; 70540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 70640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 707687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 708332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) { 709332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract payload from descriptor */ 710332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi_rx_desc(desc, cfhsi) < 0) 711332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 712332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } else { 713332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract payload */ 714332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi_rx_pld(desc, cfhsi) < 0) 715332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 716332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (piggy_desc) { 717332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Extract any payload in piggyback descriptor. */ 718332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (cfhsi_rx_desc(piggy_desc, cfhsi) < 0) 719332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto out_of_sync; 7203935600a7f341c00b0def2ed8870669ab2c05493Per Ellefsen /* Mark no embedded frame after extracting it */ 7213935600a7f341c00b0def2ed8870669ab2c05493Per Ellefsen piggy_desc->offset = 0; 722332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 723687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 724332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 725332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com /* Update state info */ 726332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state)); 727332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.state = rx_state; 728332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_ptr = rx_ptr; 729332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_len = rx_len; 730332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.pld_len = desc_pld_len; 731332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_state.piggy_desc = desc->header & CFHSI_PIGGY_DESC; 732332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 733332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (rx_buf != cfhsi->rx_buf) 734332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com swap(cfhsi->rx_buf, cfhsi->rx_flip_buf); 7355bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return; 7365bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 7375bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonout_of_sync: 73890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Out of sync.\n", __func__); 7395bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE, 7405bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi->rx_buf, CFHSI_DESC_SZ); 7415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson schedule_work(&cfhsi->out_of_sync_work); 742687b13e98addc99644002703944ec89e94287cb6Daniel Martensson} 743687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 744687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_slowpath(unsigned long arg) 745687b13e98addc99644002703944ec89e94287cb6Daniel Martensson{ 746687b13e98addc99644002703944ec89e94287cb6Daniel Martensson struct cfhsi *cfhsi = (struct cfhsi *)arg; 747687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 74890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 749687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 750687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 751687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 75240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 75340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7541c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændelandstatic void cfhsi_rx_done_cb(struct cfhsi_cb_ops *cb_ops) 75540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 75640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 75740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7581c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi = container_of(cb_ops, struct cfhsi, cb_ops); 75990de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 76040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 76140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 76240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 76340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 76440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 76540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits)) 76640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->flush_fifo_wait); 76740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 768687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 76940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 77040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up(struct work_struct *work) 77240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 77340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 77440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 77540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 77640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 77740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_up_work); 77940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 78040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 78140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 78240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 78340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) { 78440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* It happenes when wakeup is requested by 78540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * both ends at the same time. */ 78640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 7875ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 78840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 78940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 79040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate wake line. */ 7921c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_wake_up(cfhsi->ops); 79340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Start waiting.\n", 79540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 79640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 798687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 799687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait, 800687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_UP_ACK, 80140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &cfhsi->bits), ret); 80240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(ret < 0)) { 80340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 80490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n", 80540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 8065ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 80740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 8081c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_wake_down(cfhsi->ops); 80940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 81040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 8115ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson bool ca_wake = false; 8125ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson size_t fifo_occupancy = 0; 8135ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 81440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wakeup timeout */ 81590de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Timeout.\n", 81640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 8175ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8185ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check FIFO to check if modem has sent something. */ 8191c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops, 8205ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &fifo_occupancy)); 8215ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 82290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Bytes in FIFO: %u.\n", 8235ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__, (unsigned) fifo_occupancy); 8245ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8255ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check if we misssed the interrupt. */ 8261c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops, 8275ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &ca_wake)); 8285ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8295ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson if (ca_wake) { 83090de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n", 8315ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__); 8325ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8335ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */ 8345ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 8355ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8365ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Continue execution. */ 8375ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson goto wake_ack; 8385ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson } 8395ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 84040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 8411c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_wake_down(cfhsi->ops); 84240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 84340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 8445ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martenssonwake_ack: 84590de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Woken.\n", 84640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 84740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 84840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up bit. */ 84940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_AWAKE, &cfhsi->bits); 85040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 85140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 85240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Resume read operation. */ 85390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Start RX.\n", __func__); 8541c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland res = cfhsi->ops->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->ops); 855687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 856687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (WARN_ON(res < 0)) 85790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: RX err %d.\n", __func__, res); 85840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 85940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up acknowledment. */ 86040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 86140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 86340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 864ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Resume transmit if queues are not empty. */ 865ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (!cfhsi_tx_queue_len(cfhsi)) { 86690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Peer wake, start timer.\n", 86740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 86840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Start inactivity timer. */ 869ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin mod_timer(&cfhsi->inactivity_timer, 87091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland jiffies + cfhsi->cfg.inactivity_timeout); 87140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 87240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 87340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 87440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87590de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s: Host wake.\n", 87640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 87740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 87940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 88040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 88140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi); 88240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 88340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (likely(len > 0)) { 88440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 8851c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops); 88640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 88790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: TX error %d.\n", 88840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 88940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 89040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 89140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 89290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, 89340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Failed to create HSI frame: %d.\n", 89440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, len); 89540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 89640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 89740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down(struct work_struct *work) 89940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 90040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 90140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 902687b13e98addc99644002703944ec89e94287cb6Daniel Martensson size_t fifo_occupancy = 0; 903687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int retry = CFHSI_WAKE_TOUT; 90440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 90540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_down_work); 90690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", __func__); 90740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 90840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 90940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 91040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 91140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate wake line. */ 9121c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_wake_down(cfhsi->ops); 91340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 91440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 915687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 91640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait, 917687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_DOWN_ACK, 918687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &cfhsi->bits), ret); 91940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 92040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 92190de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Signalled: %ld.\n", 92240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 92340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 92440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 9255ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson bool ca_wake = true; 9265ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 92740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Timeout */ 92890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Timeout.\n", __func__); 9295ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 9305ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check if we misssed the interrupt. */ 9311c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland WARN_ON(cfhsi->ops->cfhsi_get_peer_wake(cfhsi->ops, 9325ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &ca_wake)); 9335ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson if (!ca_wake) 93490de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: CA Wake missed !.\n", 9355ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__); 93640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 93740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 938687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Check FIFO occupancy. */ 939687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (retry) { 9401c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland WARN_ON(cfhsi->ops->cfhsi_fifo_occupancy(cfhsi->ops, 941687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &fifo_occupancy)); 942687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 943687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!fifo_occupancy) 944687b13e98addc99644002703944ec89e94287cb6Daniel Martensson break; 945687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 946687b13e98addc99644002703944ec89e94287cb6Daniel Martensson set_current_state(TASK_INTERRUPTIBLE); 947687b13e98addc99644002703944ec89e94287cb6Daniel Martensson schedule_timeout(1); 948687b13e98addc99644002703944ec89e94287cb6Daniel Martensson retry--; 949687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 950687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 951687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!retry) 95290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: FIFO Timeout.\n", __func__); 953687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 954687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Clear AWAKE condition. */ 95540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 95640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 957687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Cancel pending RX requests. */ 9581c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops); 95940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 96040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9615bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic void cfhsi_out_of_sync(struct work_struct *work) 9625bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson{ 9635bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson struct cfhsi *cfhsi = NULL; 9645bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 9655bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi = container_of(work, struct cfhsi, out_of_sync_work); 9665bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 9675bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_lock(); 9685bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_close(cfhsi->ndev); 9695bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_unlock(); 9705bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson} 9715bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 9721c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændelandstatic void cfhsi_wake_up_cb(struct cfhsi_cb_ops *cb_ops) 97340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 97440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 97540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9761c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi = container_of(cb_ops, struct cfhsi, cb_ops); 97790de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 97840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 97940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 98040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 98140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_up_wait); 98240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 98340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 98440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 98540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 98640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the peer initiates. */ 98740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 98840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 98940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 99040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9911c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændelandstatic void cfhsi_wake_down_cb(struct cfhsi_cb_ops *cb_ops) 99240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 99340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 99440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9951c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi = container_of(cb_ops, struct cfhsi, cb_ops); 99690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 99740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 99840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initiating low power is only permitted by the host (us). */ 100040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 100140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_down_wait); 100240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 100340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1004ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyaginstatic void cfhsi_aggregation_tout(unsigned long arg) 1005ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin{ 1006ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin struct cfhsi *cfhsi = (struct cfhsi *)arg; 1007ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 100890de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_dbg(cfhsi->ndev, "%s.\n", 1009ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin __func__); 1010ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 1011ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_start_tx(cfhsi); 1012ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin} 1013ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 101440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev) 101540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 101640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 101740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int start_xfer = 0; 101840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int timer_active; 1019ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int prio; 102040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 102140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!dev) 102240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 102340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 102440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(dev); 102540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1026ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin switch (skb->priority) { 1027ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_BESTEFFORT: 1028ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_FILLER: 1029ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_BULK: 1030ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_BEBK; 1031ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1032ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_INTERACTIVE_BULK: 1033ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_VI; 1034ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1035ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_INTERACTIVE: 1036ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_VO; 1037ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1038ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin case TC_PRIO_CONTROL: 1039ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin default: 1040ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin prio = CFHSI_PRIO_CTL; 1041ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin break; 1042ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin } 1043ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 104440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 104540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1046ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Update aggregation statistics */ 1047ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_update_aggregation_stats(cfhsi, skb, 1); 1048ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 1049ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Queue the SKB */ 1050ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb_queue_tail(&cfhsi->qhead[prio], skb); 105140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 105240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check; xmit should not be called after unregister_netdev */ 105340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) { 105440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 105540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 105640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 105740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 105840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 105940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Send flow off if number of packets is above high water mark. */ 106040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->flow_off_sent && 106191fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi_tx_queue_len(cfhsi) > cfhsi->cfg.q_high_mark && 106240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl) { 106340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 1; 106440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF); 106540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 106640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 106740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) { 106840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_XFER; 106940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin start_xfer = 1; 107040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 107140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 107273033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin if (!start_xfer) { 1073ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Send aggregate if it is possible */ 1074ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin bool aggregate_ready = 1075ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_can_send_aggregate(cfhsi) && 1076ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin del_timer(&cfhsi->aggregation_timer) > 0; 107773033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 1078ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin if (aggregate_ready) 1079ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi_start_tx(cfhsi); 108040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 108173033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin } 108240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Delete inactivity timer if started. */ 1084ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin timer_active = del_timer_sync(&cfhsi->inactivity_timer); 108540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108673033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 108773033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin 108840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (timer_active) { 108940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf; 109040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 109140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 109240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 109340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 109440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 1095f84ea779c25dabc90956f1c329e5e5c501ea96ccRoar Førde WARN_ON(!len); 109640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 109740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 10981c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland res = cfhsi->ops->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->ops); 109940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 110090de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: TX error %d.\n", 110140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 110240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 110340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 110440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 110540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the we initiate. */ 110640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 110740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 110840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 110940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 111040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 111140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 111240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 11131c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændelandstatic const struct net_device_ops cfhsi_netdevops; 111440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 111540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_setup(struct net_device *dev) 111640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 1117ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin int i; 111840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = netdev_priv(dev); 111940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->features = 0; 112040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->type = ARPHRD_CAIF; 112140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->flags = IFF_POINTOPOINT | IFF_NOARP; 112234efc283a56adaef5756ac93065f46608674ea7dSjur Brændeland dev->mtu = CFHSI_MAX_CAIF_FRAME_SZ; 112340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->tx_queue_len = 0; 112440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->destructor = free_netdev; 11251c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland dev->netdev_ops = &cfhsi_netdevops; 1126ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin for (i = 0; i < CFHSI_PRIO_LAST; ++i) 1127ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin skb_queue_head_init(&cfhsi->qhead[i]); 112840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW; 112940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_frag = false; 113040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_stx = false; 113140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_fcs = false; 113240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = dev; 11337fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland cfhsi->cfg = hsi_default_config; 113440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 113540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113639abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.comstatic int cfhsi_open(struct net_device *ndev) 113739abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com{ 113839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com struct cfhsi *cfhsi = netdev_priv(ndev); 113939abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com int res; 114039abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 114139abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com clear_bit(CFHSI_SHUTDOWN, &cfhsi->bits); 114239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 114340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize state vaiables. */ 114440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 1145687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_DESC; 114640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 114740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set flow info */ 114840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 0; 114940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 115140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a TX buffer with the size of a HSI packet descriptors 115240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and the necessary room for CAIF payload frames. 115340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 115440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL); 115540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->tx_buf) { 115640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 115740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_tx; 115840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 115940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 116040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 116140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a RX buffer with the size of two HSI packet descriptors and 116240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * the necessary room for CAIF payload frames. 116340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 116440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL); 116540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->rx_buf) { 116640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 116740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_rx; 116840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 116940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1170332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com cfhsi->rx_flip_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL); 1171332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com if (!cfhsi->rx_flip_buf) { 1172332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com res = -ENODEV; 1173332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com goto err_alloc_rx_flip; 1174332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com } 1175332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com 1176ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Initialize aggregation timeout */ 117791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.aggregation_timeout = hsi_default_config.aggregation_timeout; 1178ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin 117928bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Initialize recieve vaiables. */ 118040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf; 118140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = CFHSI_DESC_SZ; 118240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin locks. */ 118440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi->lock); 118540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up the driver. */ 11871c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->cb_ops.tx_done_cb = cfhsi_tx_done_cb; 11881c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->cb_ops.rx_done_cb = cfhsi_rx_done_cb; 11891c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->cb_ops.wake_up_cb = cfhsi_wake_up_cb; 11901c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->cb_ops.wake_down_cb = cfhsi_wake_down_cb; 119140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 119240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize the work queues. */ 119340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up); 119440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down); 11955bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync); 119640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 119740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear all bit fields. */ 119840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 119940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 120040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 120140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 120240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 120340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create work thread. */ 12041c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->wq = create_singlethread_workqueue(cfhsi->ndev->name); 120540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->wq) { 120690de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Failed to create work queue.\n", 120740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 120840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 120940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_create_wq; 121040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 121140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize wait queues. */ 121340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_up_wait); 121440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_down_wait); 121540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->flush_fifo_wait); 121640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Setup the inactivity timer. */ 1218ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin init_timer(&cfhsi->inactivity_timer); 1219ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->inactivity_timer.data = (unsigned long)cfhsi; 1220ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->inactivity_timer.function = cfhsi_inactivity_tout; 1221687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Setup the slowpath RX timer. */ 1222687b13e98addc99644002703944ec89e94287cb6Daniel Martensson init_timer(&cfhsi->rx_slowpath_timer); 1223687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi; 1224687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath; 1225ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin /* Setup the aggregation timer. */ 1226ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin init_timer(&cfhsi->aggregation_timer); 1227ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_timer.data = (unsigned long)cfhsi; 1228ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin cfhsi->aggregation_timer.function = cfhsi_aggregation_tout; 122940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 123040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate HSI interface. */ 12311c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland res = cfhsi->ops->cfhsi_up(cfhsi->ops); 123240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 123390de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, 123440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't activate HSI interface: %d.\n", 123540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 123640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_activate; 123740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 123840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 123940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush FIFO */ 124040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi_flush_fifo(cfhsi); 124140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 124290de9bab9123d7fe905cb885c1e3ed32dc516f18Sjur Brændeland netdev_err(cfhsi->ndev, "%s: Can't flush FIFO: %d.\n", 124340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 124440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_net_reg; 124540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 124640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 124740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_net_reg: 12491c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_down(cfhsi->ops); 125040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_activate: 125140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 125240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_create_wq: 1253332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com kfree(cfhsi->rx_flip_buf); 1254332ad43f19848ed653a5e44afa8e2f4a7d34ed44sjur.brandeland@stericsson.com err_alloc_rx_flip: 125540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->rx_buf); 125640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_rx: 125740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->tx_buf); 125840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_tx: 125940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 126040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 126140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 126239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.comstatic int cfhsi_close(struct net_device *ndev) 126340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 126439abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com struct cfhsi *cfhsi = netdev_priv(ndev); 12655f614e6b7005685b3e34da0742f79389c858c7e1sjur.brandeland@stericsson.com u8 *tx_buf, *rx_buf, *flip_buf; 126640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 126740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* going to shutdown driver */ 126840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_SHUTDOWN, &cfhsi->bits); 126940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 127040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush workqueue */ 127140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin flush_workqueue(cfhsi->wq); 127240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1273687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Delete timers if pending */ 1274ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin del_timer_sync(&cfhsi->inactivity_timer); 1275687b13e98addc99644002703944ec89e94287cb6Daniel Martensson del_timer_sync(&cfhsi->rx_slowpath_timer); 1276ece367d53a5bf46cc357163c7074a6546a0ec01cDmitry Tarnyagin del_timer_sync(&cfhsi->aggregation_timer); 127740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 127840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Cancel pending RX request (if any) */ 12791c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_rx_cancel(cfhsi->ops); 128040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1281ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson /* Destroy workqueue */ 128240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 128340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 128440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Store bufferes: will be freed later. */ 128540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tx_buf = cfhsi->tx_buf; 128640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_buf = cfhsi->rx_buf; 12875f614e6b7005685b3e34da0742f79389c858c7e1sjur.brandeland@stericsson.com flip_buf = cfhsi->rx_flip_buf; 128840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush transmit queues. */ 128940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 129040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 129140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate interface */ 12921c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændeland cfhsi->ops->cfhsi_down(cfhsi->ops); 129340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 129440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Free buffers. */ 129540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(tx_buf); 129640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(rx_buf); 12975f614e6b7005685b3e34da0742f79389c858c7e1sjur.brandeland@stericsson.com kfree(flip_buf); 129839abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com return 0; 129940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 130040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1301c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic void cfhsi_uninit(struct net_device *dev) 1302c41254006377842013922fb1e407391f24c59522Sjur Brændeland{ 1303c41254006377842013922fb1e407391f24c59522Sjur Brændeland struct cfhsi *cfhsi = netdev_priv(dev); 1304c41254006377842013922fb1e407391f24c59522Sjur Brændeland ASSERT_RTNL(); 1305c41254006377842013922fb1e407391f24c59522Sjur Brændeland symbol_put(cfhsi_get_device); 1306c41254006377842013922fb1e407391f24c59522Sjur Brændeland list_del(&cfhsi->list); 1307c41254006377842013922fb1e407391f24c59522Sjur Brændeland} 1308c41254006377842013922fb1e407391f24c59522Sjur Brændeland 13091c385f1fdf6f9c66d982802cd74349c040980b50Sjur Brændelandstatic const struct net_device_ops cfhsi_netdevops = { 1310c41254006377842013922fb1e407391f24c59522Sjur Brændeland .ndo_uninit = cfhsi_uninit, 131139abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com .ndo_open = cfhsi_open, 131239abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com .ndo_stop = cfhsi_close, 131339abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com .ndo_start_xmit = cfhsi_xmit 131439abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com}; 131539abbaef19cd0a30be93794aa4773c779c3eb1f3sjur.brandeland@stericsson.com 1316c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic void cfhsi_netlink_parms(struct nlattr *data[], struct cfhsi *cfhsi) 131740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 1318c41254006377842013922fb1e407391f24c59522Sjur Brændeland int i; 131940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1320c41254006377842013922fb1e407391f24c59522Sjur Brændeland if (!data) { 1321c41254006377842013922fb1e407391f24c59522Sjur Brændeland pr_debug("no params data found\n"); 1322c41254006377842013922fb1e407391f24c59522Sjur Brændeland return; 132340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 1324c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1325c41254006377842013922fb1e407391f24c59522Sjur Brændeland i = __IFLA_CAIF_HSI_INACTIVITY_TOUT; 132691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland /* 132791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * Inactivity timeout in millisecs. Lowest possible value is 1, 132891fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland * and highest possible is NEXT_TIMER_MAX_DELTA. 132991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland */ 133091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland if (data[i]) { 133191fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland u32 inactivity_timeout = nla_get_u32(data[i]); 133291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland /* Pre-calculate inactivity timeout. */ 133391fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.inactivity_timeout = inactivity_timeout * HZ / 1000; 133491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland if (cfhsi->cfg.inactivity_timeout == 0) 133591fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.inactivity_timeout = 1; 133691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland else if (cfhsi->cfg.inactivity_timeout > NEXT_TIMER_MAX_DELTA) 133791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.inactivity_timeout = NEXT_TIMER_MAX_DELTA; 133891fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland } 1339c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1340c41254006377842013922fb1e407391f24c59522Sjur Brændeland i = __IFLA_CAIF_HSI_AGGREGATION_TOUT; 1341c41254006377842013922fb1e407391f24c59522Sjur Brændeland if (data[i]) 134291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.aggregation_timeout = nla_get_u32(data[i]); 1343c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1344c41254006377842013922fb1e407391f24c59522Sjur Brændeland i = __IFLA_CAIF_HSI_HEAD_ALIGN; 1345c41254006377842013922fb1e407391f24c59522Sjur Brændeland if (data[i]) 134691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.head_align = nla_get_u32(data[i]); 1347c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1348c41254006377842013922fb1e407391f24c59522Sjur Brændeland i = __IFLA_CAIF_HSI_TAIL_ALIGN; 1349c41254006377842013922fb1e407391f24c59522Sjur Brændeland if (data[i]) 135091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.tail_align = nla_get_u32(data[i]); 1351c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1352c41254006377842013922fb1e407391f24c59522Sjur Brændeland i = __IFLA_CAIF_HSI_QHIGH_WATERMARK; 1353c41254006377842013922fb1e407391f24c59522Sjur Brændeland if (data[i]) 135491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.q_high_mark = nla_get_u32(data[i]); 135591fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland 135691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland i = __IFLA_CAIF_HSI_QLOW_WATERMARK; 135791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland if (data[i]) 135891fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.q_low_mark = nla_get_u32(data[i]); 1359c41254006377842013922fb1e407391f24c59522Sjur Brændeland} 1360c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1361c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic int caif_hsi_changelink(struct net_device *dev, struct nlattr *tb[], 1362c41254006377842013922fb1e407391f24c59522Sjur Brændeland struct nlattr *data[]) 1363c41254006377842013922fb1e407391f24c59522Sjur Brændeland{ 1364c41254006377842013922fb1e407391f24c59522Sjur Brændeland cfhsi_netlink_parms(data, netdev_priv(dev)); 1365c41254006377842013922fb1e407391f24c59522Sjur Brændeland netdev_state_change(dev); 1366c41254006377842013922fb1e407391f24c59522Sjur Brændeland return 0; 136740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 136840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1369c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic const struct nla_policy caif_hsi_policy[__IFLA_CAIF_HSI_MAX + 1] = { 1370c41254006377842013922fb1e407391f24c59522Sjur Brændeland [__IFLA_CAIF_HSI_INACTIVITY_TOUT] = { .type = NLA_U32, .len = 4 }, 1371c41254006377842013922fb1e407391f24c59522Sjur Brændeland [__IFLA_CAIF_HSI_AGGREGATION_TOUT] = { .type = NLA_U32, .len = 4 }, 1372c41254006377842013922fb1e407391f24c59522Sjur Brændeland [__IFLA_CAIF_HSI_HEAD_ALIGN] = { .type = NLA_U32, .len = 4 }, 1373c41254006377842013922fb1e407391f24c59522Sjur Brændeland [__IFLA_CAIF_HSI_TAIL_ALIGN] = { .type = NLA_U32, .len = 4 }, 1374c41254006377842013922fb1e407391f24c59522Sjur Brændeland [__IFLA_CAIF_HSI_QHIGH_WATERMARK] = { .type = NLA_U32, .len = 4 }, 1375c41254006377842013922fb1e407391f24c59522Sjur Brændeland [__IFLA_CAIF_HSI_QLOW_WATERMARK] = { .type = NLA_U32, .len = 4 }, 137640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin}; 137740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1378c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic size_t caif_hsi_get_size(const struct net_device *dev) 1379c41254006377842013922fb1e407391f24c59522Sjur Brændeland{ 1380c41254006377842013922fb1e407391f24c59522Sjur Brændeland int i; 1381c41254006377842013922fb1e407391f24c59522Sjur Brændeland size_t s = 0; 1382c41254006377842013922fb1e407391f24c59522Sjur Brændeland for (i = __IFLA_CAIF_HSI_UNSPEC + 1; i < __IFLA_CAIF_HSI_MAX; i++) 1383c41254006377842013922fb1e407391f24c59522Sjur Brændeland s += nla_total_size(caif_hsi_policy[i].len); 1384c41254006377842013922fb1e407391f24c59522Sjur Brændeland return s; 1385c41254006377842013922fb1e407391f24c59522Sjur Brændeland} 1386c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1387c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic int caif_hsi_fill_info(struct sk_buff *skb, const struct net_device *dev) 1388c41254006377842013922fb1e407391f24c59522Sjur Brændeland{ 138991fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland struct cfhsi *cfhsi = netdev_priv(dev); 139091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland 1391c41254006377842013922fb1e407391f24c59522Sjur Brændeland if (nla_put_u32(skb, __IFLA_CAIF_HSI_INACTIVITY_TOUT, 139291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.inactivity_timeout) || 1393c41254006377842013922fb1e407391f24c59522Sjur Brændeland nla_put_u32(skb, __IFLA_CAIF_HSI_AGGREGATION_TOUT, 139491fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.aggregation_timeout) || 139591fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland nla_put_u32(skb, __IFLA_CAIF_HSI_HEAD_ALIGN, 139691fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.head_align) || 139791fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland nla_put_u32(skb, __IFLA_CAIF_HSI_TAIL_ALIGN, 139891fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.tail_align) || 1399c41254006377842013922fb1e407391f24c59522Sjur Brændeland nla_put_u32(skb, __IFLA_CAIF_HSI_QHIGH_WATERMARK, 140091fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.q_high_mark) || 1401c41254006377842013922fb1e407391f24c59522Sjur Brændeland nla_put_u32(skb, __IFLA_CAIF_HSI_QLOW_WATERMARK, 140291fa0cbc0c47930f771bf5425109956cc99c6b0bSjur Brændeland cfhsi->cfg.q_low_mark)) 1403c41254006377842013922fb1e407391f24c59522Sjur Brændeland return -EMSGSIZE; 1404c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1405c41254006377842013922fb1e407391f24c59522Sjur Brændeland return 0; 1406c41254006377842013922fb1e407391f24c59522Sjur Brændeland} 1407c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1408c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic int caif_hsi_newlink(struct net *src_net, struct net_device *dev, 1409c41254006377842013922fb1e407391f24c59522Sjur Brændeland struct nlattr *tb[], struct nlattr *data[]) 141040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 141140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 14127fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland struct cfhsi_ops *(*get_ops)(void); 141340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1414c41254006377842013922fb1e407391f24c59522Sjur Brændeland ASSERT_RTNL(); 1415c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1416c41254006377842013922fb1e407391f24c59522Sjur Brændeland cfhsi = netdev_priv(dev); 1417c41254006377842013922fb1e407391f24c59522Sjur Brændeland cfhsi_netlink_parms(data, cfhsi); 1418c41254006377842013922fb1e407391f24c59522Sjur Brændeland dev_net_set(cfhsi->ndev, src_net); 1419c41254006377842013922fb1e407391f24c59522Sjur Brændeland 14207fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland get_ops = symbol_get(cfhsi_get_ops); 14217fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland if (!get_ops) { 14227fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland pr_err("%s: failed to get the cfhsi_ops\n", __func__); 14237fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland return -ENODEV; 14247fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland } 14257fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland 14267fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland /* Assign the HSI device. */ 14277fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland cfhsi->ops = (*get_ops)(); 14287fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland if (!cfhsi->ops) { 14297fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland pr_err("%s: failed to get the cfhsi_ops\n", __func__); 14307fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland goto err; 14317fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland } 14327fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland 14337fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland /* Assign the driver to this HSI device. */ 14347fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland cfhsi->ops->cb_ops = &cfhsi->cb_ops; 14357fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland if (register_netdevice(dev)) { 14367fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland pr_warn("%s: caif_hsi device registration failed\n", __func__); 14377fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland goto err; 14387fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland } 14397fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland /* Add CAIF HSI device to list. */ 14407fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland list_add_tail(&cfhsi->list, &cfhsi_list); 14417fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland 1442c41254006377842013922fb1e407391f24c59522Sjur Brændeland return 0; 14437fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændelanderr: 14447fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland symbol_put(cfhsi_get_ops); 14457fa8ad6df71f4778b23e17b0106aaef02b518684Sjur Brændeland return -ENODEV; 144640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 144740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1448c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic struct rtnl_link_ops caif_hsi_link_ops __read_mostly = { 1449c41254006377842013922fb1e407391f24c59522Sjur Brændeland .kind = "cfhsi", 1450c41254006377842013922fb1e407391f24c59522Sjur Brændeland .priv_size = sizeof(struct cfhsi), 1451c41254006377842013922fb1e407391f24c59522Sjur Brændeland .setup = cfhsi_setup, 1452c41254006377842013922fb1e407391f24c59522Sjur Brændeland .maxtype = __IFLA_CAIF_HSI_MAX, 1453c41254006377842013922fb1e407391f24c59522Sjur Brændeland .policy = caif_hsi_policy, 1454c41254006377842013922fb1e407391f24c59522Sjur Brændeland .newlink = caif_hsi_newlink, 1455c41254006377842013922fb1e407391f24c59522Sjur Brændeland .changelink = caif_hsi_changelink, 1456c41254006377842013922fb1e407391f24c59522Sjur Brændeland .get_size = caif_hsi_get_size, 1457c41254006377842013922fb1e407391f24c59522Sjur Brændeland .fill_info = caif_hsi_fill_info, 1458c41254006377842013922fb1e407391f24c59522Sjur Brændeland}; 1459c41254006377842013922fb1e407391f24c59522Sjur Brændeland 1460c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic void __exit cfhsi_exit_module(void) 146140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 1462c41254006377842013922fb1e407391f24c59522Sjur Brændeland struct list_head *list_node; 1463c41254006377842013922fb1e407391f24c59522Sjur Brændeland struct list_head *n; 1464c41254006377842013922fb1e407391f24c59522Sjur Brændeland struct cfhsi *cfhsi; 146540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1466c41254006377842013922fb1e407391f24c59522Sjur Brændeland rtnl_link_unregister(&caif_hsi_link_ops); 146740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1468c41254006377842013922fb1e407391f24c59522Sjur Brændeland rtnl_lock(); 1469c41254006377842013922fb1e407391f24c59522Sjur Brændeland list_for_each_safe(list_node, n, &cfhsi_list) { 1470c41254006377842013922fb1e407391f24c59522Sjur Brændeland cfhsi = list_entry(list_node, struct cfhsi, list); 1471c41254006377842013922fb1e407391f24c59522Sjur Brændeland unregister_netdev(cfhsi->ndev); 147240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 1473c41254006377842013922fb1e407391f24c59522Sjur Brændeland rtnl_unlock(); 1474c41254006377842013922fb1e407391f24c59522Sjur Brændeland} 147540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1476c41254006377842013922fb1e407391f24c59522Sjur Brændelandstatic int __init cfhsi_init_module(void) 1477c41254006377842013922fb1e407391f24c59522Sjur Brændeland{ 1478c41254006377842013922fb1e407391f24c59522Sjur Brændeland return rtnl_link_register(&caif_hsi_link_ops); 147940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 148040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 148140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_init(cfhsi_init_module); 148240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_exit(cfhsi_exit_module); 1483