caif_hsi.c revision 5bbed92d3d8dab1f28945eec9fb15b6f50bf8669
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 12140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_wake_up(cfhsi->dev); 12240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 12340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 12440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't wake up HSI interface: %d.\n", 12540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 12640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return ret; 12740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 12840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 12940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 13040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 13140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &fifo_occupancy); 13240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 13340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 13440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't get FIFO occupancy: %d.\n", 13540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 13640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 13740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!fifo_occupancy) 13840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* No more data, exitting normally */ 13940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 14040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 14140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin fifo_occupancy = min(sizeof(buffer), fifo_occupancy); 14240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 14340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = cfhsi->dev->cfhsi_rx(buffer, fifo_occupancy, 14440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev); 14540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret) { 14640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits); 14740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 14840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't read data: %d.\n", 14940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 15040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 15140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 15240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 15340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = 5 * HZ; 154687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->flush_fifo_wait, 15540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin !test_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits), ret); 15640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 15740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 15840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 15940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't wait for flush complete: %d.\n", 16040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 16140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 16240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 16340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = -ETIMEDOUT; 16440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_warn(&cfhsi->ndev->dev, 16540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: timeout waiting for flush complete.\n", 16640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 16740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 16840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 16940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (1); 17040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 17240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return ret; 17440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 17540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 17640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_tx_frm(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 17740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 17840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 17940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int pld_len = 0; 18040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 18140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 18240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_dequeue(&cfhsi->qhead); 18440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 18540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 18640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 18794230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com /* Clear offset. */ 18894230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com desc->offset = 0; 18994230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com 19040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can embed a CAIF frame. */ 19140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (skb->len < CFHSI_MAX_EMB_FRM_SZ) { 19240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 19340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int hpad = 0; 19440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int tpad = 0; 19540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 19640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 19740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 19840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 19940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 20040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 20140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 20240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if frame still fits with added alignment. */ 20340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((skb->len + hpad + tpad) <= CFHSI_MAX_EMB_FRM_SZ) { 20440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pemb = desc->emb_frm; 20540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->offset = CFHSI_DESC_SHORT_SZ; 20640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pemb = (u8)(hpad - 1); 20740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pemb += hpad; 20840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 20940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 21040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 21140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 21240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 21340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in embedded CAIF frame. */ 21440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pemb, skb->len); 21540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 21640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 21740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 21894230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com } 21940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create payload CAIF frames. */ 22140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 22240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 22340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct caif_payload_info *info; 22440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int hpad = 0; 22540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int tpad = 0; 22640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 22740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 22840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_dequeue(&cfhsi->qhead); 22940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb) 23140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin break; 23240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate needed head alignment and tail alignment. */ 23440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin info = (struct caif_payload_info *)&skb->cb; 23540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin hpad = 1 + PAD_POW2((info->hdr_len + 1), hsi_head_align); 23740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tpad = PAD_POW2((skb->len + hpad), hsi_tail_align); 23840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 23940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill in CAIF frame length in descriptor. */ 24040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = hpad + skb->len + tpad; 24140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Fill head padding information. */ 24340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin *pfrm = (u8)(hpad - 1); 24440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += hpad; 24540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 24640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 24740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_packets++; 24840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.tx_bytes += skb->len; 24940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Copy in CAIF frame. */ 25140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_copy_bits(skb, 0, pfrm, skb->len); 25240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update payload length. */ 25440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pld_len += desc->cffrm_len[nfrms]; 25540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 25640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update frame pointer. */ 25740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += skb->len + tpad; 25840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin consume_skb(skb); 25940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = NULL; 26040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 26140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update number of frames. */ 26240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 26340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 26440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 26540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unused length fields should be zero-filled (according to SPEC). */ 26640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS) { 26740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->cffrm_len[nfrms] = 0x0000; 26840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 26940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 27040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check if we can piggy-back another descriptor. */ 27240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb = skb_peek(&cfhsi->qhead); 27340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (skb) 27440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header |= CFHSI_PIGGY_DESC; 27540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 27640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc->header &= ~CFHSI_PIGGY_DESC; 27740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 27840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return CFHSI_DESC_SZ + pld_len; 27940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 28040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 281687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_tx_done(struct cfhsi *cfhsi) 28240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 28340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = NULL; 28440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len = 0; 28540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 28640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 287687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__); 28840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 28940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 29040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 29140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc = (struct cfhsi_desc *)cfhsi->tx_buf; 29340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 29440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin do { 29540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 29640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Send flow on if flow off has been previously signalled 29740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and number of packets is below low water mark. 29840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 29940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 30040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->flow_off_sent && 30140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->qhead.qlen <= cfhsi->q_low_mark && 30240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl) { 30340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 0; 30540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, ON); 30640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 30740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 30840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 30940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 310fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin do { 311fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 312fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin if (!len) { 313fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 314fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin if (unlikely(skb_peek(&cfhsi->qhead))) { 315fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 316fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin continue; 317fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } 318fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 319fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin /* Start inactivity timer. */ 320fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin mod_timer(&cfhsi->timer, 32128bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 322fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 323fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin goto done; 324fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } 325fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin } while (!len); 32640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 32740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 32840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 32940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 33040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n", 33140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 33240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 33340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } while (res < 0); 334fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin 335fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagindone: 336fe47f1250805438fa06580c9ce6d37bc4bc595d2Dmitry Tarnyagin return; 33740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 33840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 33940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_tx_done_cb(struct cfhsi_drv *drv) 34040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 34140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 34240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 34340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 34440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 34540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 34640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 34740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 34840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 349687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_tx_done(cfhsi); 35040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 35140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3525bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_desc(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 35340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 35440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int xfer_sz = 0; 35540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 35640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 35740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 35840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 35940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((desc->header & ~CFHSI_PIGGY_DESC) || 36040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ)) { 36140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n", 36240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 3635bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 36440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 36540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 36640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for embedded CAIF frame. */ 36740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->offset) { 36840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 36940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 370687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int len = 0; 37140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = ((u8 *)desc) + desc->offset; 37240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 37340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove offset padding. */ 37440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *pfrm + 1; 37540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 37640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 37740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pfrm; 37840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pfrm+1)) << 8) & 0xFF00; 37940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 38040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 3815bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frame. */ 3825bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 3835bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n", 3845bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 3855bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 3865bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 38740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 38840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 389687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 390687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 391687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n", 392687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 393687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 39440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 39540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 39640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 39740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 39840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pfrm, len); 39940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 40040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 40140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 40240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 40340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 40440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 40540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * We are called from a arch specific platform device. 40640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Unfortunately we don't know what context we're 40740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * running in. 40840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 40940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 41040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 41140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 41240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 41340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 41440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 41540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 41640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 41740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 41840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 41940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Calculate transfer length. */ 42040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 42140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 42240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += *plen; 42340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 42440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 42540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 42640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 42740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Check for piggy-backed descriptor. */ 42840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc->header & CFHSI_PIGGY_DESC) 42940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin xfer_sz += CFHSI_DESC_SZ; 43040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4315bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if ((xfer_sz % 4) || (xfer_sz > (CFHSI_BUF_SZ_RX - CFHSI_DESC_SZ))) { 43240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, 43340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Invalid payload len: %d, ignored.\n", 43440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, xfer_sz); 4355bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 43640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 43740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return xfer_sz; 43840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 43940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4405bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic int cfhsi_rx_pld(struct cfhsi_desc *desc, struct cfhsi *cfhsi) 44140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 44240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int rx_sz = 0; 44340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int nfrms = 0; 44440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u16 *plen = NULL; 44540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pfrm = NULL; 44640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 44740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check header and offset. */ 44840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON((desc->header & ~CFHSI_PIGGY_DESC) || 44940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->offset > CFHSI_MAX_EMB_FRM_SZ))) { 45040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: Invalid descriptor.\n", 45140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 4525bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 45340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 45440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 45540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set frame pointer to start of payload. */ 45640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm = desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ; 45740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen = desc->cffrm_len; 458687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 459687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Skip already processed frames. */ 460687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (nfrms < cfhsi->rx_state.nfrms) { 461687b13e98addc99644002703944ec89e94287cb6Daniel Martensson pfrm += *plen; 462687b13e98addc99644002703944ec89e94287cb6Daniel Martensson rx_sz += *plen; 463687b13e98addc99644002703944ec89e94287cb6Daniel Martensson plen++; 464687b13e98addc99644002703944ec89e94287cb6Daniel Martensson nfrms++; 465687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 466687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 467687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Parse payload. */ 46840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin while (nfrms < CFHSI_MAX_PKTS && *plen) { 46940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct sk_buff *skb; 47040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *dst = NULL; 47140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *pcffrm = NULL; 472687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int len = 0; 47340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* CAIF frame starts after head padding. */ 47540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pcffrm = pfrm + *pfrm + 1; 47640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 47740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Read length of CAIF frame (little endian). */ 47840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = *pcffrm; 47940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len |= ((*(pcffrm + 1)) << 8) & 0xFF00; 48040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len += 2; /* Add FCS fields. */ 48140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 4825bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson /* Sanity check length of CAIF frames. */ 4835bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (unlikely(len > CFHSI_MAX_CAIF_FRAME_SZ)) { 4845bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Invalid length.\n", 4855bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson __func__); 4865bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return -EPROTO; 4875bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 4885bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 48940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Allocate SKB (OK even in IRQ context). */ 490687b13e98addc99644002703944ec89e94287cb6Daniel Martensson skb = alloc_skb(len + 1, GFP_ATOMIC); 491687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!skb) { 492687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Out of memory !\n", 493687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 494687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.nfrms = nfrms; 495687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return -ENOMEM; 49640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 49740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin caif_assert(skb != NULL); 49840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 49940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dst = skb_put(skb, len); 50040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy(dst, pcffrm, len); 50140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 50240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->protocol = htons(ETH_P_CAIF); 50340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_reset_mac_header(skb); 50440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb->dev = cfhsi->ndev; 50540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 50640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 50740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * We're called from a platform device, 50840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and don't know the context we're running in. 50940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 51040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (in_interrupt()) 51140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx(skb); 51240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 51340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_rx_ni(skb); 51440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 51540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update network statistics. */ 51640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_packets++; 51740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_bytes += len; 51840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 51940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pfrm += *plen; 52040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_sz += *plen; 52140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin plen++; 52240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin nfrms++; 52340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 52440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 52540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return rx_sz; 52640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 52740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 528687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_done(struct cfhsi *cfhsi) 52940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 53040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 53140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int desc_pld_len = 0; 53240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = NULL; 53340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 53440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin desc = (struct cfhsi_desc *)cfhsi->rx_buf; 53540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 536687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s\n", __func__); 53740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 53840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 53940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 54040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 54140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Update inactivity timer if pending. */ 54273033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_lock_bh(&cfhsi->lock); 54328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin mod_timer_pending(&cfhsi->timer, 54428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 54573033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 54640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 547687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (cfhsi->rx_state.state == CFHSI_RX_STATE_DESC) { 5485bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson desc_pld_len = cfhsi_rx_desc(desc, cfhsi); 549687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (desc_pld_len == -ENOMEM) 550687b13e98addc99644002703944ec89e94287cb6Daniel Martensson goto restart; 5515bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (desc_pld_len == -EPROTO) 5525bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 55340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 55440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int pld_len; 55540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 556687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!cfhsi->rx_state.piggy_desc) { 5575bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson pld_len = cfhsi_rx_pld(desc, cfhsi); 558687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (pld_len == -ENOMEM) 559687b13e98addc99644002703944ec89e94287cb6Daniel Martensson goto restart; 5605bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (pld_len == -EPROTO) 5615bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 562687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.pld_len = pld_len; 563687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } else { 564687b13e98addc99644002703944ec89e94287cb6Daniel Martensson pld_len = cfhsi->rx_state.pld_len; 565687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 56640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 56740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if ((pld_len > 0) && (desc->header & CFHSI_PIGGY_DESC)) { 56840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *piggy_desc; 56940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin piggy_desc = (struct cfhsi_desc *) 57040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin (desc->emb_frm + CFHSI_MAX_EMB_FRM_SZ + 57140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin pld_len); 572687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.piggy_desc = true; 57340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 57440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Extract piggy-backed descriptor. */ 5755bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson desc_pld_len = cfhsi_rx_desc(piggy_desc, cfhsi); 576687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (desc_pld_len == -ENOMEM) 577687b13e98addc99644002703944ec89e94287cb6Daniel Martensson goto restart; 57840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 57940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 58040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Copy needed information from the piggy-backed 58140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * descriptor to the descriptor in the start. 58240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 58340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin memcpy((u8 *)desc, (u8 *)piggy_desc, 58440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin CFHSI_DESC_SHORT_SZ); 58540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 5865bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson if (desc_pld_len == -EPROTO) 5875bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson goto out_of_sync; 5885bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson } 589687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 590687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 591687b13e98addc99644002703944ec89e94287cb6Daniel Martensson memset(&cfhsi->rx_state, 0, sizeof(cfhsi->rx_state)); 59240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (desc_pld_len) { 593687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_PAYLOAD; 59440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf + CFHSI_DESC_SZ; 59540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = desc_pld_len; 59640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 597687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_DESC; 59840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf; 59940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = CFHSI_DESC_SZ; 60040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 60140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 60240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_AWAKE, &cfhsi->bits)) { 60340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 60440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n", 60540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 60640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, 60740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev); 60840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 60940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: RX error %d.\n", 61040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 61140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_errors++; 61240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev->stats.rx_dropped++; 61340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 61440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 615687b13e98addc99644002703944ec89e94287cb6Daniel Martensson return; 616687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 617687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonrestart: 618687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (++cfhsi->rx_state.retries > CFHSI_MAX_RX_RETRIES) { 619687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: No memory available " 620687b13e98addc99644002703944ec89e94287cb6Daniel Martensson "in %d iterations.\n", 621687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__, CFHSI_MAX_RX_RETRIES); 622687b13e98addc99644002703944ec89e94287cb6Daniel Martensson BUG(); 623687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 624687b13e98addc99644002703944ec89e94287cb6Daniel Martensson mod_timer(&cfhsi->rx_slowpath_timer, jiffies + 1); 6255bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson return; 6265bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 6275bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonout_of_sync: 6285bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Out of sync.\n", __func__); 6295bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE, 6305bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi->rx_buf, CFHSI_DESC_SZ); 6315bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson schedule_work(&cfhsi->out_of_sync_work); 632687b13e98addc99644002703944ec89e94287cb6Daniel Martensson} 633687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 634687b13e98addc99644002703944ec89e94287cb6Daniel Martenssonstatic void cfhsi_rx_slowpath(unsigned long arg) 635687b13e98addc99644002703944ec89e94287cb6Daniel Martensson{ 636687b13e98addc99644002703944ec89e94287cb6Daniel Martensson struct cfhsi *cfhsi = (struct cfhsi *)arg; 637687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 638687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s.\n", 639687b13e98addc99644002703944ec89e94287cb6Daniel Martensson __func__); 640687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 641687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 64240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 64340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_rx_done_cb(struct cfhsi_drv *drv) 64540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 64640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi; 64740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 64840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 64940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 65040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 65140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 65240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 65340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 65440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 65540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_and_clear_bit(CFHSI_FLUSH_FIFO, &cfhsi->bits)) 65640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->flush_fifo_wait); 65740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin else 658687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi_rx_done(cfhsi); 65940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 66040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 66140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up(struct work_struct *work) 66240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 66340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 66440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 66540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 66640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 66740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 66840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_up_work); 66940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 67140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 67240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 67340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(test_bit(CFHSI_AWAKE, &cfhsi->bits))) { 67440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* It happenes when wakeup is requested by 67540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * both ends at the same time. */ 67640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 67740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 67840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 67940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 68040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate wake line. */ 68140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_up(cfhsi->dev); 68240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 68340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Start waiting.\n", 68440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 68540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 68640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 687687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 688687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = wait_event_interruptible_timeout(cfhsi->wake_up_wait, 689687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_UP_ACK, 69040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin &cfhsi->bits), ret); 69140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (unlikely(ret < 0)) { 69240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 69340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_info(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n", 69440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 69540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 69640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 69740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 69840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 69940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wakeup timeout */ 70040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n", 70140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 70240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 70340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 70440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 70540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 70640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Woken.\n", 70740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 70840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 70940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up bit. */ 71040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_AWAKE, &cfhsi->bits); 71140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 71240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 71340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Resume read operation. */ 714687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s: Start RX.\n", __func__); 715687b13e98addc99644002703944ec89e94287cb6Daniel Martensson res = cfhsi->dev->cfhsi_rx(cfhsi->rx_ptr, cfhsi->rx_len, cfhsi->dev); 716687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 717687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (WARN_ON(res < 0)) 718687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: RX err %d.\n", __func__, res); 71940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 72040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear power up acknowledment. */ 72140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 72240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 72340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 72440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 72540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Resume transmit if queue is not empty. */ 72640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!skb_peek(&cfhsi->qhead)) { 72740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Peer wake, start timer.\n", 72840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 72940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Start inactivity timer. */ 73040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin mod_timer(&cfhsi->timer, 73128bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin jiffies + cfhsi->inactivity_timeout); 73240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 73340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 73440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 73540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 73640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s: Host wake.\n", 73740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 73840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 73940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 74040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 74140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 74240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm((struct cfhsi_desc *)cfhsi->tx_buf, cfhsi); 74340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 74440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (likely(len > 0)) { 74540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 74640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 74740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 74840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n", 74940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 75040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 75140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 75240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 75340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, 75440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: Failed to create HSI frame: %d.\n", 75540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, len); 75640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 75740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 75840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 75940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down(struct work_struct *work) 76040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 76140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin long ret; 76240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 763687b13e98addc99644002703944ec89e94287cb6Daniel Martensson size_t fifo_occupancy = 0; 764687b13e98addc99644002703944ec89e94287cb6Daniel Martensson int retry = CFHSI_WAKE_TOUT; 76540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 76640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(work, struct cfhsi, wake_down_work); 767687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_dbg(&cfhsi->ndev->dev, "%s.\n", __func__); 76840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 76940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 77040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 77140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate wake line. */ 77340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_wake_down(cfhsi->dev); 77440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 77540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Wait for acknowledge. */ 776687b13e98addc99644002703944ec89e94287cb6Daniel Martensson ret = CFHSI_WAKE_TOUT; 77740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ret = wait_event_interruptible_timeout(cfhsi->wake_down_wait, 778687b13e98addc99644002703944ec89e94287cb6Daniel Martensson test_and_clear_bit(CFHSI_WAKE_DOWN_ACK, 779687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &cfhsi->bits), ret); 78040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (ret < 0) { 78140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Interrupted by signal. */ 78240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_info(&cfhsi->ndev->dev, "%s: Signalled: %ld.\n", 78340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, ret); 78440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 78540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else if (!ret) { 78640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Timeout */ 787687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: Timeout.\n", __func__); 78840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 78940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 790687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Check FIFO occupancy. */ 791687b13e98addc99644002703944ec89e94287cb6Daniel Martensson while (retry) { 792687b13e98addc99644002703944ec89e94287cb6Daniel Martensson WARN_ON(cfhsi->dev->cfhsi_fifo_occupancy(cfhsi->dev, 793687b13e98addc99644002703944ec89e94287cb6Daniel Martensson &fifo_occupancy)); 794687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 795687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!fifo_occupancy) 796687b13e98addc99644002703944ec89e94287cb6Daniel Martensson break; 797687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 798687b13e98addc99644002703944ec89e94287cb6Daniel Martensson set_current_state(TASK_INTERRUPTIBLE); 799687b13e98addc99644002703944ec89e94287cb6Daniel Martensson schedule_timeout(1); 800687b13e98addc99644002703944ec89e94287cb6Daniel Martensson retry--; 801687b13e98addc99644002703944ec89e94287cb6Daniel Martensson } 802687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 803687b13e98addc99644002703944ec89e94287cb6Daniel Martensson if (!retry) 804687b13e98addc99644002703944ec89e94287cb6Daniel Martensson dev_err(&cfhsi->ndev->dev, "%s: FIFO Timeout.\n", __func__); 805687b13e98addc99644002703944ec89e94287cb6Daniel Martensson 806687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Clear AWAKE condition. */ 80740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 80840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 809687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Cancel pending RX requests. */ 810687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev); 81140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 81240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 81340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 8145bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martenssonstatic void cfhsi_out_of_sync(struct work_struct *work) 8155bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson{ 8165bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson struct cfhsi *cfhsi = NULL; 8175bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 8185bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson cfhsi = container_of(work, struct cfhsi, out_of_sync_work); 8195bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 8205bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_lock(); 8215bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson dev_close(cfhsi->ndev); 8225bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson rtnl_unlock(); 8235bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson} 8245bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson 82540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_up_cb(struct cfhsi_drv *drv) 82640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 82740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 82840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 82940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 83040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 83140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 83240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 83340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 83440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_up_wait); 83540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 83640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (test_bit(CFHSI_SHUTDOWN, &cfhsi->bits)) 83740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return; 83840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 83940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the peer initiates. */ 84040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 84140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 84240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 84340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 84440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_wake_down_cb(struct cfhsi_drv *drv) 84540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 84640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 84740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 84840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = container_of(drv, struct cfhsi, drv); 84940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_dbg(&cfhsi->ndev->dev, "%s.\n", 85040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 85140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 85240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initiating low power is only permitted by the host (us). */ 85340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 85440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin wake_up_interruptible(&cfhsi->wake_down_wait); 85540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 85640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 85740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_xmit(struct sk_buff *skb, struct net_device *dev) 85840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 85940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 86040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int start_xfer = 0; 86140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int timer_active; 86240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!dev) 86440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 86540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(dev); 86740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 86840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_bh(&cfhsi->lock); 86940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_queue_tail(&cfhsi->qhead, skb); 87140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Sanity check; xmit should not be called after unregister_netdev */ 87340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(test_bit(CFHSI_SHUTDOWN, &cfhsi->bits))) { 87440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock_bh(&cfhsi->lock); 87540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 87640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -EINVAL; 87740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 87840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 87940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Send flow off if number of packets is above high water mark. */ 88040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->flow_off_sent && 88140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->qhead.qlen > cfhsi->q_high_mark && 88240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl) { 88340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 1; 88440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.flowctrl(cfhsi->ndev, OFF); 88540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 88640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 88740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->tx_state == CFHSI_TX_STATE_IDLE) { 88840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_XFER; 88940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin start_xfer = 1; 89040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 89140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89273033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin if (!start_xfer) { 89373033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 89440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 89573033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin } 89640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 89740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Delete inactivity timer if started. */ 89840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin timer_active = del_timer_sync(&cfhsi->timer); 89940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 90073033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin spin_unlock_bh(&cfhsi->lock); 90173033c987a8bd0b080509063bb7c130b8941ad73Dmitry Tarnyagin 90240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (timer_active) { 90340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_desc *desc = (struct cfhsi_desc *)cfhsi->tx_buf; 90440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int len; 90540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 90640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 90740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create HSI frame. */ 90840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin len = cfhsi_tx_frm(desc, cfhsi); 90940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin BUG_ON(!len); 91040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 91140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up new transfer. */ 91240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_tx(cfhsi->tx_buf, len, cfhsi->dev); 91340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (WARN_ON(res < 0)) { 91440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, "%s: TX error %d.\n", 91540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 91640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 91740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 91840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } else { 91940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Schedule wake up work queue if the we initiate. */ 92040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!test_and_set_bit(CFHSI_WAKE_UP, &cfhsi->bits)) 92140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin queue_work(cfhsi->wq, &cfhsi->wake_up_work); 92240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 92340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 92540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 92640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 92740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_open(struct net_device *dev) 92840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 92940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_wake_queue(dev); 93040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 93140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 93240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 93340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 93440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int cfhsi_close(struct net_device *dev) 93540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 93640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_stop_queue(dev); 93740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 93840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 93940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 94040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 94140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic const struct net_device_ops cfhsi_ops = { 94240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .ndo_open = cfhsi_open, 94340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .ndo_stop = cfhsi_close, 94440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .ndo_start_xmit = cfhsi_xmit 94540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin}; 94640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 94740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void cfhsi_setup(struct net_device *dev) 94840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 94940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = netdev_priv(dev); 95040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->features = 0; 95140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->netdev_ops = &cfhsi_ops; 95240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->type = ARPHRD_CAIF; 95340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->flags = IFF_POINTOPOINT | IFF_NOARP; 95440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->mtu = CFHSI_MAX_PAYLOAD_SZ; 95540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->tx_queue_len = 0; 95640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->destructor = free_netdev; 95740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin skb_queue_head_init(&cfhsi->qhead); 95840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.link_select = CAIF_LINK_HIGH_BANDW; 95940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_frag = false; 96040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_stx = false; 96140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->cfdev.use_fcs = false; 96240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = dev; 96340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 96440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 96540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginint cfhsi_probe(struct platform_device *pdev) 96640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 96740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 96840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct net_device *ndev; 96940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_dev *dev; 97040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int res; 97140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 97240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin ndev = alloc_netdev(sizeof(struct cfhsi), "cfhsi%d", cfhsi_setup); 9737ac2ed0ceeafa130f85aa947b271b571c68b9e75Joe Perches if (!ndev) 97440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -ENODEV; 97540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 97640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = netdev_priv(ndev); 97740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->ndev = ndev; 97840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->pdev = pdev; 97940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 98040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize state vaiables. */ 98140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_state = CFHSI_TX_STATE_IDLE; 982687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_state.state = CFHSI_RX_STATE_DESC; 98340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 98440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set flow info */ 98540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->flow_off_sent = 0; 98640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->q_low_mark = LOW_WATER_MARK; 98740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->q_high_mark = HIGH_WATER_MARK; 98840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 98940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Assign the HSI device. */ 99040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev = (struct cfhsi_dev *)pdev->dev.platform_data; 99140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev = dev; 99240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Assign the driver to this HSI device. */ 99440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev->drv = &cfhsi->drv; 99540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 99640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 99740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a TX buffer with the size of a HSI packet descriptors 99840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * and the necessary room for CAIF payload frames. 99940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 100040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->tx_buf = kzalloc(CFHSI_BUF_SZ_TX, GFP_KERNEL); 100140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->tx_buf) { 100240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 100340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_tx; 100440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 100540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 100640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* 100740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * Allocate a RX buffer with the size of two HSI packet descriptors and 100840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin * the necessary room for CAIF payload frames. 100940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin */ 101040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_buf = kzalloc(CFHSI_BUF_SZ_RX, GFP_KERNEL); 101140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->rx_buf) { 101240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 101340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_alloc_rx; 101440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 101540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 101628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Pre-calculate inactivity timeout. */ 101728bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin if (inactivity_timeout != -1) { 101828bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = 101928bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin inactivity_timeout * HZ / 1000; 102028bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin if (!cfhsi->inactivity_timeout) 102128bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = 1; 102228bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin else if (cfhsi->inactivity_timeout > NEXT_TIMER_MAX_DELTA) 102328bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA; 102428bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin } else { 102528bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin cfhsi->inactivity_timeout = NEXT_TIMER_MAX_DELTA; 102628bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin } 102728bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin 102828bd2049428202cb3bc982536ed3de3c69ae120aDmitry Tarnyagin /* Initialize recieve vaiables. */ 102940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_ptr = cfhsi->rx_buf; 103040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->rx_len = CFHSI_DESC_SZ; 103140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 103240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin locks. */ 103340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi->lock); 103440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 103540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Set up the driver. */ 103640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->drv.tx_done_cb = cfhsi_tx_done_cb; 103740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->drv.rx_done_cb = cfhsi_rx_done_cb; 103894230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com cfhsi->drv.wake_up_cb = cfhsi_wake_up_cb; 103994230febe47f82331f9493c4fd61085e2a6bf756sjur.brandeland@stericsson.com cfhsi->drv.wake_down_cb = cfhsi_wake_down_cb; 104040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 104140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize the work queues. */ 104240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_up_work, cfhsi_wake_up); 104340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin INIT_WORK(&cfhsi->wake_down_work, cfhsi_wake_down); 10445bbed92d3d8dab1f28945eec9fb15b6f50bf8669Daniel Martensson INIT_WORK(&cfhsi->out_of_sync_work, cfhsi_out_of_sync); 104540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 104640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Clear all bit fields. */ 104740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP_ACK, &cfhsi->bits); 104840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_DOWN_ACK, &cfhsi->bits); 104940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_WAKE_UP, &cfhsi->bits); 105040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin clear_bit(CFHSI_AWAKE, &cfhsi->bits); 105140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 105240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Create work thread. */ 105340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->wq = create_singlethread_workqueue(pdev->name); 105440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (!cfhsi->wq) { 105540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&ndev->dev, "%s: Failed to create work queue.\n", 105640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__); 105740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = -ENODEV; 105840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_create_wq; 105940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 106040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 106140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize wait queues. */ 106240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_up_wait); 106340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->wake_down_wait); 106440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_waitqueue_head(&cfhsi->flush_fifo_wait); 106540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 106640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Setup the inactivity timer. */ 106740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin init_timer(&cfhsi->timer); 106840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->timer.data = (unsigned long)cfhsi; 106940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->timer.function = cfhsi_inactivity_tout; 1070687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Setup the slowpath RX timer. */ 1071687b13e98addc99644002703944ec89e94287cb6Daniel Martensson init_timer(&cfhsi->rx_slowpath_timer); 1072687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.data = (unsigned long)cfhsi; 1073687b13e98addc99644002703944ec89e94287cb6Daniel Martensson cfhsi->rx_slowpath_timer.function = cfhsi_rx_slowpath; 107440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 107540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Add CAIF HSI device to list. */ 107640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 107740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_add_tail(&cfhsi->list, &cfhsi_list); 107840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 107940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Activate HSI interface. */ 108140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi->dev->cfhsi_up(cfhsi->dev); 108240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 108340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&cfhsi->ndev->dev, 108440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin "%s: can't activate HSI interface: %d.\n", 108540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 108640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_activate; 108740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 108840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 108940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush FIFO */ 109040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = cfhsi_flush_fifo(cfhsi); 109140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 109240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&ndev->dev, "%s: Can't flush FIFO: %d.\n", 109340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 109440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_net_reg; 109540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 109640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 109740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Register network device. */ 109840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin res = register_netdev(ndev); 109940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (res) { 110040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev_err(&ndev->dev, "%s: Registration error: %d.\n", 110140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin __func__, res); 110240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_net_reg; 110340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 110440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_stop_queue(ndev); 110640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 110840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 110940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_net_reg: 111040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_down(cfhsi->dev); 111140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_activate: 111240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 111340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_create_wq: 111440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->rx_buf); 111540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_rx: 111640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(cfhsi->tx_buf); 111740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_alloc_tx: 111840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin free_netdev(ndev); 111940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 112040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return res; 112140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 112240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1123ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martenssonstatic void cfhsi_shutdown(struct cfhsi *cfhsi) 112440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 112540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin u8 *tx_buf, *rx_buf; 112640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 112740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Stop TXing */ 112840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin netif_tx_stop_all_queues(cfhsi->ndev); 112940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* going to shutdown driver */ 113140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin set_bit(CFHSI_SHUTDOWN, &cfhsi->bits); 113240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 113340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush workqueue */ 113440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin flush_workqueue(cfhsi->wq); 113540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1136687b13e98addc99644002703944ec89e94287cb6Daniel Martensson /* Delete timers if pending */ 113740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin del_timer_sync(&cfhsi->timer); 1138687b13e98addc99644002703944ec89e94287cb6Daniel Martensson del_timer_sync(&cfhsi->rx_slowpath_timer); 113940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 114040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Cancel pending RX request (if any) */ 114140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_rx_cancel(cfhsi->dev); 114240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 1143ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson /* Destroy workqueue */ 114440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin destroy_workqueue(cfhsi->wq); 114540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 114640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Store bufferes: will be freed later. */ 114740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin tx_buf = cfhsi->tx_buf; 114840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin rx_buf = cfhsi->rx_buf; 114940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Flush transmit queues. */ 115140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi_abort_tx(cfhsi); 115240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Deactivate interface */ 115440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi->dev->cfhsi_down(cfhsi->dev); 115540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Finally unregister the network device. */ 115740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin unregister_netdev(cfhsi->ndev); 115840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 115940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Free buffers. */ 116040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(tx_buf); 116140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin kfree(rx_buf); 116240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 116340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 116440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginint cfhsi_remove(struct platform_device *pdev) 116540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 116640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *list_node; 116740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *n; 116840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 116940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi_dev *dev; 117040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 117140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin dev = (struct cfhsi_dev *)pdev->dev.platform_data; 117240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 117340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_for_each_safe(list_node, n, &cfhsi_list) { 117440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = list_entry(list_node, struct cfhsi, list); 117540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Find the corresponding device. */ 117640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (cfhsi->dev == dev) { 117740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove from list. */ 117840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_del(list_node); 117940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 118040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Shutdown driver. */ 1182ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson cfhsi_shutdown(cfhsi); 118340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 118440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return 0; 118540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 118640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 118740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 118840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return -ENODEV; 118940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 119040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 119140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstruct platform_driver cfhsi_plat_drv = { 119240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .probe = cfhsi_probe, 119340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .remove = cfhsi_remove, 119440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .driver = { 119540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .name = "cfhsi", 119640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin .owner = THIS_MODULE, 119740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin }, 119840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin}; 119940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 120040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic void __exit cfhsi_exit_module(void) 120140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 120240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *list_node; 120340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct list_head *n; 120440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin struct cfhsi *cfhsi = NULL; 120540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 120640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 120740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_for_each_safe(list_node, n, &cfhsi_list) { 120840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin cfhsi = list_entry(list_node, struct cfhsi, list); 120940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Remove from list. */ 121140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin list_del(list_node); 121240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 121340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Shutdown driver. */ 1215ca63f8c7512acbd1171bbabefc7a7765ce117939Daniel Martensson cfhsi_shutdown(cfhsi); 121640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 121740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock(&cfhsi_list_lock); 121840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 121940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_unlock(&cfhsi_list_lock); 122040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 122140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Unregister platform driver. */ 122240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin platform_driver_unregister(&cfhsi_plat_drv); 122340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 122440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 122540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginstatic int __init cfhsi_init_module(void) 122640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin{ 122740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin int result; 122840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 122940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Initialize spin lock. */ 123040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin spin_lock_init(&cfhsi_list_lock); 123140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 123240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin /* Register platform driver. */ 123340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin result = platform_driver_register(&cfhsi_plat_drv); 123440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin if (result) { 123540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin printk(KERN_ERR "Could not register platform HSI driver: %d.\n", 123640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin result); 123740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin goto err_dev_register; 123840d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin } 123940d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124040d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return result; 124140d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124240d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin err_dev_register: 124340d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin return result; 124440d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin} 124540d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyagin 124640d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_init(cfhsi_init_module); 124740d69043fce579378eb185d31445b6ff66abbd93Dmitry.Tarnyaginmodule_exit(cfhsi_exit_module); 1248