140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copyright (C) ST-Ericsson AB 2010 340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Contact: Sjur Brendeland / sjur.brandeland@stericsson.com 440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Author: Daniel Martensson / daniel.martensson@stericsson.com 540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Dmitry.Tarnyagin / dmitry.tarnyagin@stericsson.com 640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * License terms: GNU General Public License (GPL) version 2. 740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/init.h> 1040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/module.h> 1140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/device.h> 1240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <linux/platform_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> 215bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson#include <linux/rtnetlink.h> 2240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <net/caif/caif_layer.h> 2340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#include <net/caif/caif_hsi.h> 2440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 2540d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_LICENSE("GPL"); 2640d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>"); 2740d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_DESCRIPTION("CAIF HSI driver"); 2840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 2940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* Returns the number of padding bytes for alignment. */ 3040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define PAD_POW2(x, pow) ((((x)&((pow)-1)) == 0) ? 0 :\ 3140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (((pow)-((x)&((pow)-1))))) 3240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyaginstatic int inactivity_timeout = 1000; 3428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyaginmodule_param(inactivity_timeout, int, S_IRUGO | S_IWUSR); 3528bd2049428202cb3bc982536ed3de3c69ae120aDmitry TarnyaginMODULE_PARM_DESC(inactivity_timeout, "Inactivity timeout on HSI, ms."); 3628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin 3740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 3840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * HSI padding options. 3940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Warning: must be a base of 2 (& operation used) and can not be zero ! 4040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 4140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_head_align = 4; 4240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_head_align, int, S_IRUGO); 4340d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_head_align, "HSI head alignment."); 4440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_tail_align = 4; 4640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_tail_align, int, S_IRUGO); 4740d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_tail_align, "HSI tail alignment."); 4840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 5040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * HSI link layer flowcontrol thresholds. 5140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Warning: A high threshold value migth increase throughput but it will at 5240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * the same time prevent channel prioritization and increase the risk of 5340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * flooding the modem. The high threshold should be above the low. 5440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 5540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_high_threshold = 100; 5640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_high_threshold, int, S_IRUGO); 5740d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_high_threshold, "HSI high threshold (FLOW OFF)."); 5840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int hsi_low_threshold = 50; 6040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_param(hsi_low_threshold, int, S_IRUGO); 6140d69043fce579378eb185d31445b6ff66abbd93Dmitry.TarnyaginMODULE_PARM_DESC(hsi_low_threshold, "HSI high threshold (FLOW ON)."); 6240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 6340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define ON 1 6440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define OFF 0 6540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 6640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin/* 6740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Threshold values for the HSI packet queue. Flowcontrol will be asserted 6840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * when the number of packets exceeds HIGH_WATER_MARK. It will not be 6940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * de-asserted before the number of packets drops below LOW_WATER_MARK. 7040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 7140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define LOW_WATER_MARK hsi_low_threshold 7240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin#define HIGH_WATER_MARK hsi_high_threshold 7340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic LIST_HEAD(cfhsi_list); 7540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic spinlock_t cfhsi_list_lock; 7640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 7740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_inactivity_tout(unsigned long arg) 7840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 7940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = (struct cfhsi *)arg; 8040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 8240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 8340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule power down work queue. */ 8540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 8640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_down_work); 8740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 8840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_abort_tx(struct cfhsi *cfhsi) 9040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 9140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 9240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin for (;;) { 9440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 9540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_dequeue(&cfhsi->qhead); 9640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 9740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 9840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 9940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_errors++; 10040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_dropped++; 10140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 10240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree_skb(skb); 10340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 10440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 10540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 10628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin mod_timer(&cfhsi->timer, 10728bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 10840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 10940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 11040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 11140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_flush_fifo(struct cfhsi *cfhsi) 11240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 11340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin char buffer[32]; /* Any reasonable value */ 11440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin size_t fifo_occupancy; 11540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int ret; 11640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 11740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 11840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 11940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 12040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 12140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 12240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &fifo_occupancy); 12340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 12440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 12540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't get FIFO occupancy: %d.\n", 12640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 12740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 12840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!fifo_occupancy) 12940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* No more data, exitting normally */ 13040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 13140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 13240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin fifo_occupancy = min(sizeof(buffer), fifo_occupancy); 13340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 13440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_rx(buffer, fifo_occupancy, 13540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev); 13640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 13740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 13840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 13940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't read data: %d.\n", 14040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 14140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 14240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 14340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 14440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = 5 * HZ; 145687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->flush_fifo_wait, 14640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret); 14740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 14840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 14940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 15040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't wait for flush complete: %d.\n", 15140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 15240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 15340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 15440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = -ETIMEDOUT; 15540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 15640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: timeout waiting for flush complete.\n", 15740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 15840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 15940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 16040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (1); 16140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 16240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return ret; 16340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 16440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 16540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 16640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 16740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 16840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int pld_len = 0; 16940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 17040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 17140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_dequeue(&cfhsi->qhead); 17340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 17440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 17540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17694230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com /* Clear offset. */ 17794230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com desc->offset = 0; 17894230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com 17940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can embed a CAIF frame. */ 18040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (skb->len < CFHSI_MAX_EMB_FRM_SZ) { 18140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 18240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int hpad = 0; 18340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int tpad = 0; 18440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 18640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 18740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 18940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 19040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 19140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if frame still fits with added alignment. */ 19240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) { 19340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pemb = desc->emb_frm; 19440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->offset = CFHSI_DESC_SHORT_SZ; 19540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pemb = (u8)(hpad - 1); 19640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pemb += hpad; 19740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 19840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 19940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 20040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 20140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 20240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in embedded CAIF frame. */ 20340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pemb, skb->len); 20440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 20540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 20640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 20794230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com } 20840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 20940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create payload CAIF frames. */ 21040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 21140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 21240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 21340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int hpad = 0; 21440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int tpad = 0; 21540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 21740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_dequeue(&cfhsi->qhead); 21840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 22040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 22140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 22340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 22440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 22640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 22740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill in CAIF frame length in descriptor. */ 22940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = hpad + skb->len + tpad; 23040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill head padding information. */ 23240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pfrm = (u8)(hpad - 1); 23340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += hpad; 23440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 23640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 23740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 23840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in CAIF frame. */ 24040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pfrm, skb->len); 24140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update payload length. */ 24340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pld_len += desc->cffrm_len[nfrms]; 24440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update frame pointer. */ 24640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += skb->len + tpad; 24740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 24840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 24940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update number of frames. */ 25140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 25240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 25340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unused length fields should be zero-filled (according to SPEC). */ 25540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 25640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = 0x0000; 25740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 25840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 25940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 26040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can piggy-back another descriptor. */ 26140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_peek(&cfhsi->qhead); 26240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (skb) 26340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header |= CFHSI_PIGGY_DESC; 26440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 26540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header &= ~CFHSI_PIGGY_DESC; 26640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 26740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return CFHSI_DESC_SZ + pld_len; 26840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 26940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 270687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_tx_done(struct cfhsi *cfhsi) 27140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 27240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = NULL; 27340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len = 0; 27440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 27540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 276687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__); 27740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 27940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 28040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc = (struct cfhsi_desc *)cfhsi->tx_buf; 28240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 28440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 28540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Send flow on if flow off has been previously signalled 28640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and number of packets is below low water mark. 28740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 28840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 28940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->flow_off_sent && 29040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->qhead.qlen <= cfhsi->q_low_mark && 29140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl) { 29240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 0; 29440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, ON); 29540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 29640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 29740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 299fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin do { 300fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 301fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin if (!len) { 302fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 303fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin if (unlikely(skb_peek(&cfhsi->qhead))) { 304fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 305fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin continue; 306fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } 307fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 308fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin /* Start inactivity timer. */ 309fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin mod_timer(&cfhsi->timer, 31028bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 311fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 312fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin goto done; 313fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } 314fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } while (!len); 31540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 31640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 31740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 31840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 31940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n", 32040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 32140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 32240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (res < 0); 323fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin 324fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagindone: 325fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin return; 32640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 32740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 32840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_tx_done_cb(struct cfhsi_drv *drv) 32940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 33040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 33140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 33340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 33440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 33540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 33740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 338687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_tx_done(cfhsi); 33940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 34040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 34240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 34340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int xfer_sz = 0; 34440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 34540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 34640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 34740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 34840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((desc->header & ~CFHSI_PIGGY_DESC) || 34940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) { 35040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n", 35140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 3525bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 35340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 35440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 35540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for embedded CAIF frame. */ 35640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->offset) { 35740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 35840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 359687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int len = 0; 36040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = ((u8 *)desc) + desc->offset; 36140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 36240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove offset padding. */ 36340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *pfrm + 1; 36440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 36540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 36640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pfrm; 36740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pfrm+1)) << 8) & 0xFF00; 36840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 36940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3705bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frame. */ 3715bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 3725bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n", 3735bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 3745bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 3755bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 37640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 37740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 378687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 379687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 380687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n", 381687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 382687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 38340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 38440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 38540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 38640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 38740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pfrm, len); 38840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 38940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 39040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 39140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 39240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 39340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 39440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * We are called from a arch specific platform device. 39540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Unfortunately we don't know what context we're 39640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * running in. 39740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 39840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 39940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 40040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 40140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 40240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 40340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 40440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 40540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 40640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 40740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 40840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate transfer length. */ 40940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 41040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 41140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += *plen; 41240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 41340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 41440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 41540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 41640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for piggy-backed descriptor. */ 41740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->header & CFHSI_PIGGY_DESC) 41840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += CFHSI_DESC_SZ; 41940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4205bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) { 42140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, 42240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Invalid payload len: %d, ignored.\n", 42340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, xfer_sz); 4245bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 42540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 42640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return xfer_sz; 42740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 42840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4295bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 43040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 43140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int rx_sz = 0; 43240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 43340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 43440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 43540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 43640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check header and offset. */ 43740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) || 43840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) { 43940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n", 44040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 4415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 44240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 44340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 44440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set frame pointer to start of payload. */ 44540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 44640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 447687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 448687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Skip already processed frames. */ 449687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (nfrms < cfhsi->rx_state.nfrms) { 450687b13e98addc99644002703944ec89e94287cb6Daniel Martensson pfrm += *plen; 451687b13e98addc99644002703944ec89e94287cb6Daniel Martensson rx_sz += *plen; 452687b13e98addc99644002703944ec89e94287cb6Daniel Martensson plen++; 453687b13e98addc99644002703944ec89e94287cb6Daniel Martensson nfrms++; 454687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 455687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 456687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Parse payload. */ 45740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 45840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 45940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 46040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pcffrm = NULL; 461687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int len = 0; 46240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 46340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* CAIF frame starts after head padding. */ 46440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pcffrm = pfrm + *pfrm + 1; 46540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 46640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 46740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pcffrm; 46840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pcffrm + 1)) << 8) & 0xFF00; 46940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 47040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4715bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frames. */ 4725bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 4735bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n", 4745bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 4755bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 4765bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 4775bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 47840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 479687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 480687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 481687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n", 482687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 483687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.nfrms = nfrms; 484687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 48540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 48640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 48740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 48840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 48940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pcffrm, len); 49040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 49140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 49240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 49340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 49440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 49540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 49640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * We're called from a platform device, 49740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and don't know the context we're running in. 49840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 49940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 50040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 50140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 50240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 50340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 50440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 50540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 50640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 50740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 50840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *plen; 50940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_sz += *plen; 51040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 51140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 51240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 51340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 51440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return rx_sz; 51540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 51640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 517687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_done(struct cfhsi *cfhsi) 51840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 51940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 52040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int desc_pld_len = 0; 52140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = NULL; 52240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 52340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc = (struct cfhsi_desc *)cfhsi->rx_buf; 52440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 525687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s\n", __func__); 52640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 52740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 52840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 52940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 53040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update inactivity timer if pending. */ 53173033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 53228bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin mod_timer_pending(&cfhsi->timer, 53328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 53473033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 53540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 536687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) { 5375bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson desc_pld_len = cfhsi_rx_desc(desc, cfhsi); 538687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (desc_pld_len == -ENOMEM) 539687b13e98addc99644002703944ec89e94287cb6Daniel Martensson goto restart; 5405bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (desc_pld_len == -EPROTO) 5415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 54240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 54340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int pld_len; 54440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 545687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!cfhsi->rx_state.piggy_desc) { 5465bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson pld_len = cfhsi_rx_pld(desc, cfhsi); 547687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (pld_len == -ENOMEM) 548687b13e98addc99644002703944ec89e94287cb6Daniel Martensson goto restart; 5495bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (pld_len == -EPROTO) 5505bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 551687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.pld_len = pld_len; 552687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } else { 553687b13e98addc99644002703944ec89e94287cb6Daniel Martensson pld_len = cfhsi->rx_state.pld_len; 554687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 55540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 55640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((pld_len > 0) && (desc->header & CFHSI_PIGGY_DESC)) { 55740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *piggy_desc; 55840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin piggy_desc = (struct cfhsi_desc *) 55940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ + 56040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pld_len); 561687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.piggy_desc = true; 56240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 56340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Extract piggy-backed descriptor. */ 5645bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson desc_pld_len = cfhsi_rx_desc(piggy_desc, cfhsi); 565687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (desc_pld_len == -ENOMEM) 566687b13e98addc99644002703944ec89e94287cb6Daniel Martensson goto restart; 56740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 56840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 56940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copy needed information from the piggy-backed 57040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * descriptor to the descriptor in the start. 57140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 57240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy((u8 *)desc, (u8 *)piggy_desc, 57340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin CFHSI_DESC_SHORT_SZ); 57440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5755bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (desc_pld_len == -EPROTO) 5765bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 5775bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 578687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 579687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 580687b13e98addc99644002703944ec89e94287cb6Daniel Martensson memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state)); 58140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc_pld_len) { 582687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_PAYLOAD; 58340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf + CFHSI_DESC_SZ; 58440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = desc_pld_len; 58540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 586687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_DESC; 58740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf; 58840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = CFHSI_DESC_SZ; 58940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 59040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 59140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) { 59240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 59340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n", 59440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 59540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, 59640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev); 59740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 59840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n", 59940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 60040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_errors++; 60140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_dropped++; 60240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 60340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 604687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return; 605687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 606687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonrestart: 607687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (++cfhsi->rx_state.retries > CFHSI_MAX_RX_RETRIES) { 608687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: No memory available " 609687b13e98addc99644002703944ec89e94287cb6Daniel Martensson "in %d iterations.\n", 610687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__, CFHSI_MAX_RX_RETRIES); 611687b13e98addc99644002703944ec89e94287cb6Daniel Martensson BUG(); 612687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 613687b13e98addc99644002703944ec89e94287cb6Daniel Martensson mod_timer(&cfhsi->rx_slowpath_timer, jiffies + 1); 6145bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return; 6155bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 6165bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonout_of_sync: 6175bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Out of sync.\n", __func__); 6185bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE, 6195bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi->rx_buf, CFHSI_DESC_SZ); 6205bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson schedule_work(&cfhsi->out_of_sync_work); 621687b13e98addc99644002703944ec89e94287cb6Daniel Martensson} 622687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 623687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_slowpath(unsigned long arg) 624687b13e98addc99644002703944ec89e94287cb6Daniel Martensson{ 625687b13e98addc99644002703944ec89e94287cb6Daniel Martensson struct cfhsi *cfhsi = (struct cfhsi *)arg; 626687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 627687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s.\n", 628687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 629687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 630687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 63140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 63240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 63340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_rx_done_cb(struct cfhsi_drv *drv) 63440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 63540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 63640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 63740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 63840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 63940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 64040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 64240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 64340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits)) 64540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->flush_fifo_wait); 64640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 647687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 64840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 64940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 65040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up(struct work_struct *work) 65140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 65240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 65340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 65440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 65540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 65640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 65740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_up_work); 65840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 65940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 66040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 66140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 66240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) { 66340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* It happenes when wakeup is requested by 66440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * both ends at the same time. */ 66540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 6665ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 66740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 66840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 66940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate wake line. */ 67140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_up(cfhsi->dev); 67240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Start waiting.\n", 67440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 67540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 677687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 678687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait, 679687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_UP_ACK, 68040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &cfhsi->bits), ret); 68140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(ret < 0)) { 68240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 6835ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n", 68440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 6855ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 68640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 68740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 68840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 68940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 6905ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson bool ca_wake = false; 6915ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson size_t fifo_occupancy = 0; 6925ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 69340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wakeup timeout */ 69440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n", 69540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 6965ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 6975ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check FIFO to check if modem has sent something. */ 6985ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 6995ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &fifo_occupancy)); 7005ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 7015ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Bytes in FIFO: %u.\n", 7025ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__, (unsigned) fifo_occupancy); 7035ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 7045ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check if we misssed the interrupt. */ 7055ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev, 7065ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &ca_wake)); 7075ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 7085ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson if (ca_wake) { 7095ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: CA Wake missed !.\n", 7105ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__); 7115ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 7125ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Clear the CFHSI_WAKE_UP_ACK bit to prevent race. */ 7135ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 7145ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 7155ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Continue execution. */ 7165ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson goto wake_ack; 7175ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson } 7185ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 71940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 72040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 72140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 72240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 7235ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martenssonwake_ack: 72440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Woken.\n", 72540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 72640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 72740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up bit. */ 72840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_AWAKE, &cfhsi->bits); 72940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 73040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 73140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Resume read operation. */ 732687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n", __func__); 733687b13e98addc99644002703944ec89e94287cb6Daniel Martensson res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->dev); 734687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 735687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (WARN_ON(res < 0)) 736687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: RX err %d.\n", __func__, res); 73740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 73840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up acknowledment. */ 73940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 74040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 74140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 74240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 74340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Resume transmit if queue is not empty. */ 74440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb_peek(&cfhsi->qhead)) { 74540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n", 74640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 74740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Start inactivity timer. */ 74840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin mod_timer(&cfhsi->timer, 74928bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 75040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 75140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 75240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 75340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 75440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Host wake.\n", 75540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 75640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 75740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 75840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 75940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 76040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi); 76140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 76240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (likely(len > 0)) { 76340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 76440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 76540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 76640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n", 76740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 76840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 76940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 77040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 77140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, 77240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Failed to create HSI frame: %d.\n", 77340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, len); 77440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 77540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 77640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down(struct work_struct *work) 77840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 77940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 78040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 781687b13e98addc99644002703944ec89e94287cb6Daniel Martensson size_t fifo_occupancy = 0; 782687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int retry = CFHSI_WAKE_TOUT; 78340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 78440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_down_work); 785687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__); 78640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 78740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 78840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 78940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate wake line. */ 79140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 79240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 79340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 794687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 79540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait, 796687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_DOWN_ACK, 797687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &cfhsi->bits), ret); 79840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 79940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 8005ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n", 80140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 80240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 80340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 8045ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson bool ca_wake = true; 8055ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 80640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Timeout */ 807687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n", __func__); 8085ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson 8095ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson /* Check if we misssed the interrupt. */ 8105ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_get_peer_wake(cfhsi->dev, 8115ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson &ca_wake)); 8125ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson if (!ca_wake) 8135ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: CA Wake missed !.\n", 8145ea2ef5f8b006ff9e970327e7fea78f1f5841c44Daniel Martensson __func__); 81540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 81640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 817687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Check FIFO occupancy. */ 818687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (retry) { 819687b13e98addc99644002703944ec89e94287cb6Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 820687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &fifo_occupancy)); 821687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 822687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!fifo_occupancy) 823687b13e98addc99644002703944ec89e94287cb6Daniel Martensson break; 824687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 825687b13e98addc99644002703944ec89e94287cb6Daniel Martensson set_current_state(TASK_INTERRUPTIBLE); 826687b13e98addc99644002703944ec89e94287cb6Daniel Martensson schedule_timeout(1); 827687b13e98addc99644002703944ec89e94287cb6Daniel Martensson retry--; 828687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 829687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 830687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!retry) 831687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: FIFO Timeout.\n", __func__); 832687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 833687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Clear AWAKE condition. */ 83440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 83540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 836687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Cancel pending RX requests. */ 837687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev); 83840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 83940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 84040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8415bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic void cfhsi_out_of_sync(struct work_struct *work) 8425bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson{ 8435bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson struct cfhsi *cfhsi = NULL; 8445bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 8455bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi = container_of(work, struct cfhsi, out_of_sync_work); 8465bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 8475bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_lock(); 8485bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_close(cfhsi->ndev); 8495bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_unlock(); 8505bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson} 8515bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 85240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up_cb(struct cfhsi_drv *drv) 85340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 85440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 85540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 85640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 85740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 85840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 85940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 86140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_up_wait); 86240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 86440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 86540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the peer initiates. */ 86740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 86840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 86940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 87040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down_cb(struct cfhsi_drv *drv) 87240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 87340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 87440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 87640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 87740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 87840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initiating low power is only permitted by the host (us). */ 88040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 88140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_down_wait); 88240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 88340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 88440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev) 88540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 88640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 88740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int start_xfer = 0; 88840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int timer_active; 88940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!dev) 89140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 89240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(dev); 89440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 89640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_queue_tail(&cfhsi->qhead, skb); 89840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check; xmit should not be called after unregister_netdev */ 90040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) { 90140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 90240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 90340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 90440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 90540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 90640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Send flow off if number of packets is above high water mark. */ 90740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->flow_off_sent && 90840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->qhead.qlen > cfhsi->q_high_mark && 90940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl) { 91040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 1; 91140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF); 91240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 91340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 91440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) { 91540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_XFER; 91640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin start_xfer = 1; 91740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 91840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 91973033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin if (!start_xfer) { 92073033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 92140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 92273033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin } 92340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Delete inactivity timer if started. */ 92540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin timer_active = del_timer_sync(&cfhsi->timer); 92640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92773033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 92873033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin 92940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (timer_active) { 93040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf; 93140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 93240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 93340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 93440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 93540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 936f84ea779c25dabc90956f1c329e5e5c501ea96ccRoar Førde WARN_ON(!len); 93740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 93840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 93940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 94040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 94140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n", 94240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 94340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 94440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 94540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 94640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the we initiate. */ 94740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 94840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 94940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 95040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 95140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 95240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 95340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 95440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_open(struct net_device *dev) 95540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 95640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_wake_queue(dev); 95740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 95840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 95940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 96040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 96140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_close(struct net_device *dev) 96240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 96340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_stop_queue(dev); 96440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 96540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 96640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 96740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 96840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic const struct net_device_ops cfhsi_ops = { 96940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .ndo_open = cfhsi_open, 97040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .ndo_stop = cfhsi_close, 97140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .ndo_start_xmit = cfhsi_xmit 97240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin}; 97340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 97440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_setup(struct net_device *dev) 97540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 97640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = netdev_priv(dev); 97740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->features = 0; 97840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->netdev_ops = &cfhsi_ops; 97940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->type = ARPHRD_CAIF; 98040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->flags = IFF_POINTOPOINT | IFF_NOARP; 98140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->mtu = CFHSI_MAX_PAYLOAD_SZ; 98240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->tx_queue_len = 0; 98340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->destructor = free_netdev; 98440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_queue_head_init(&cfhsi->qhead); 98540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW; 98640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_frag = false; 98740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_stx = false; 98840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_fcs = false; 98940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = dev; 99040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 99140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginint cfhsi_probe(struct platform_device *pdev) 99340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 99440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 99540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct net_device *ndev; 99640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_dev *dev; 99740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 99840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup); 10007ac2ed0ceeafa130f85aa947b271b571c68b9e75Joe Perches if (!ndev) 100140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -ENODEV; 100240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 100340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(ndev); 100440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = ndev; 100540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->pdev = pdev; 100640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 100740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize state vaiables. */ 100840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 1009687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_DESC; 101040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 101140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set flow info */ 101240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 0; 101340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->q_low_mark = LOW_WATER_MARK; 101440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->q_high_mark = HIGH_WATER_MARK; 101540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 101640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Assign the HSI device. */ 101740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev = (struct cfhsi_dev *)pdev->dev.platform_data; 101840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev = dev; 101940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 102040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Assign the driver to this HSI device. */ 102140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->drv = &cfhsi->drv; 102240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 102340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 102440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a TX buffer with the size of a HSI packet descriptors 102540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and the necessary room for CAIF payload frames. 102640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 102740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL); 102840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->tx_buf) { 102940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 103040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_tx; 103140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 103240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 103340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 103440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a RX buffer with the size of two HSI packet descriptors and 103540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * the necessary room for CAIF payload frames. 103640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 103740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL); 103840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->rx_buf) { 103940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 104040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_rx; 104140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 104240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 104328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Pre-calculate inactivity timeout. */ 104428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin if (inactivity_timeout != -1) { 104528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = 104628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin inactivity_timeout * HZ / 1000; 104728bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin if (!cfhsi->inactivity_timeout) 104828bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = 1; 104928bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin else if (cfhsi->inactivity_timeout > NEXT_TIMER_MAX_DELTA) 105028bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA; 105128bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin } else { 105228bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA; 105328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin } 105428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin 105528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Initialize recieve vaiables. */ 105640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf; 105740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = CFHSI_DESC_SZ; 105840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 105940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin locks. */ 106040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi->lock); 106140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 106240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up the driver. */ 106340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb; 106440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb; 106594230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb; 106694230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb; 106740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 106840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize the work queues. */ 106940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up); 107040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down); 10715bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync); 107240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 107340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear all bit fields. */ 107440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 107540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 107640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 107740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 107840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 107940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create work thread. */ 108040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->wq = create_singlethread_workqueue(pdev->name); 108140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->wq) { 108240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&ndev->dev, "%s: Failed to create work queue.\n", 108340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 108440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 108540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_create_wq; 108640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 108740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize wait queues. */ 108940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_up_wait); 109040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_down_wait); 109140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->flush_fifo_wait); 109240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 109340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Setup the inactivity timer. */ 109440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_timer(&cfhsi->timer); 109540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->timer.data = (unsigned long)cfhsi; 109640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->timer.function = cfhsi_inactivity_tout; 1097687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Setup the slowpath RX timer. */ 1098687b13e98addc99644002703944ec89e94287cb6Daniel Martensson init_timer(&cfhsi->rx_slowpath_timer); 1099687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi; 1100687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath; 110140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Add CAIF HSI device to list. */ 110340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 110440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_add_tail(&cfhsi->list, &cfhsi_list); 110540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 110640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate HSI interface. */ 110840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_up(cfhsi->dev); 110940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 111040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, 111140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't activate HSI interface: %d.\n", 111240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 111340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_activate; 111440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 111540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 111640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush FIFO */ 111740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi_flush_fifo(cfhsi); 111840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 111940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&ndev->dev, "%s: Can't flush FIFO: %d.\n", 112040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 112140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_net_reg; 112240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 112340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 112440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Register network device. */ 112540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = register_netdev(ndev); 112640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 112740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&ndev->dev, "%s: Registration error: %d.\n", 112840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 112940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_net_reg; 113040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 113140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_stop_queue(ndev); 113340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 113540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_net_reg: 113740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_down(cfhsi->dev); 113840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_activate: 113940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 114040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_create_wq: 114140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->rx_buf); 114240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_rx: 114340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->tx_buf); 114440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_tx: 114540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin free_netdev(ndev); 114640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 114740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 114840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 114940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1150ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martenssonstatic void cfhsi_shutdown(struct cfhsi *cfhsi) 115140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 115240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *tx_buf, *rx_buf; 115340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Stop TXing */ 115540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_tx_stop_all_queues(cfhsi->ndev); 115640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* going to shutdown driver */ 115840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_SHUTDOWN, &cfhsi->bits); 115940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 116040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush workqueue */ 116140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin flush_workqueue(cfhsi->wq); 116240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1163687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Delete timers if pending */ 116440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin del_timer_sync(&cfhsi->timer); 1165687b13e98addc99644002703944ec89e94287cb6Daniel Martensson del_timer_sync(&cfhsi->rx_slowpath_timer); 116640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 116740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Cancel pending RX request (if any) */ 116840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev); 116940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1170ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson /* Destroy workqueue */ 117140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 117240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 117340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Store bufferes: will be freed later. */ 117440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tx_buf = cfhsi->tx_buf; 117540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_buf = cfhsi->rx_buf; 117640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 117740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush transmit queues. */ 117840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 117940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate interface */ 118140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_down(cfhsi->dev); 118240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Finally unregister the network device. */ 118440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin unregister_netdev(cfhsi->ndev); 118540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Free buffers. */ 118740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(tx_buf); 118840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(rx_buf); 118940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 119040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 119140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginint cfhsi_remove(struct platform_device *pdev) 119240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 119340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *list_node; 119440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *n; 119540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 119640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_dev *dev; 119740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 119840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev = (struct cfhsi_dev *)pdev->dev.platform_data; 119940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 120040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_for_each_safe(list_node, n, &cfhsi_list) { 120140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = list_entry(list_node, struct cfhsi, list); 120240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Find the corresponding device. */ 120340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->dev == dev) { 120440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove from list. */ 120540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_del(list_node); 120640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 120740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 120840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Shutdown driver. */ 1209ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson cfhsi_shutdown(cfhsi); 121040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 121240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 121340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 121440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 121540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -ENODEV; 121640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 121740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstruct platform_driver cfhsi_plat_drv = { 121940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .probe = cfhsi_probe, 122040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .remove = cfhsi_remove, 122140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .driver = { 122240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .name = "cfhsi", 122340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .owner = THIS_MODULE, 122440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin }, 122540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin}; 122640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 122740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void __exit cfhsi_exit_module(void) 122840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 122940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *list_node; 123040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *n; 123140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 123240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 123340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 123440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_for_each_safe(list_node, n, &cfhsi_list) { 123540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = list_entry(list_node, struct cfhsi, list); 123640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 123740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove from list. */ 123840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_del(list_node); 123940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 124040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Shutdown driver. */ 1242ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson cfhsi_shutdown(cfhsi); 124340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 124540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 124640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 124740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unregister platform driver. */ 124940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin platform_driver_unregister(&cfhsi_plat_drv); 125040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 125140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 125240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int __init cfhsi_init_module(void) 125340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 125440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int result; 125540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 125640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin lock. */ 125740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi_list_lock); 125840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 125940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Register platform driver. */ 126040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin result = platform_driver_register(&cfhsi_plat_drv); 126140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (result) { 126240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin printk(KERN_ERR "Could not register platform HSI driver: %d.\n", 126340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin result); 126440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_dev_register; 126540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 126640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 126740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return result; 126840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 126940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_dev_register: 127040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return result; 127140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 127240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 127340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_init(cfhsi_init_module); 127440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_exit(cfhsi_exit_module); 1275