cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

jmb38x_ms.c (25615B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3 *  jmb38x_ms.c - JMicron jmb38x MemoryStick card reader
      4 *
      5 *  Copyright (C) 2008 Alex Dubov <oakad@yahoo.com>
      6 */
      7
      8#include <linux/spinlock.h>
      9#include <linux/interrupt.h>
     10#include <linux/pci.h>
     11#include <linux/dma-mapping.h>
     12#include <linux/delay.h>
     13#include <linux/highmem.h>
     14#include <linux/memstick.h>
     15#include <linux/slab.h>
     16#include <linux/module.h>
     17
     18#define DRIVER_NAME "jmb38x_ms"
     19
     20static bool no_dma;
     21module_param(no_dma, bool, 0644);
     22
     23enum {
     24	DMA_ADDRESS       = 0x00,
     25	BLOCK             = 0x04,
     26	DMA_CONTROL       = 0x08,
     27	TPC_P0            = 0x0c,
     28	TPC_P1            = 0x10,
     29	TPC               = 0x14,
     30	HOST_CONTROL      = 0x18,
     31	DATA              = 0x1c,
     32	STATUS            = 0x20,
     33	INT_STATUS        = 0x24,
     34	INT_STATUS_ENABLE = 0x28,
     35	INT_SIGNAL_ENABLE = 0x2c,
     36	TIMER             = 0x30,
     37	TIMER_CONTROL     = 0x34,
     38	PAD_OUTPUT_ENABLE = 0x38,
     39	PAD_PU_PD         = 0x3c,
     40	CLOCK_DELAY       = 0x40,
     41	ADMA_ADDRESS      = 0x44,
     42	CLOCK_CONTROL     = 0x48,
     43	LED_CONTROL       = 0x4c,
     44	VERSION           = 0x50
     45};
     46
     47struct jmb38x_ms_host {
     48	struct jmb38x_ms        *chip;
     49	void __iomem            *addr;
     50	spinlock_t              lock;
     51	struct tasklet_struct   notify;
     52	int                     id;
     53	char                    host_id[32];
     54	int                     irq;
     55	unsigned int            block_pos;
     56	unsigned long           timeout_jiffies;
     57	struct timer_list       timer;
     58	struct memstick_host	*msh;
     59	struct memstick_request *req;
     60	unsigned char           cmd_flags;
     61	unsigned char           io_pos;
     62	unsigned char           ifmode;
     63	unsigned int            io_word[2];
     64};
     65
     66struct jmb38x_ms {
     67	struct pci_dev        *pdev;
     68	int                   host_cnt;
     69	struct memstick_host  *hosts[];
     70};
     71
     72#define BLOCK_COUNT_MASK       0xffff0000
     73#define BLOCK_SIZE_MASK        0x00000fff
     74
     75#define DMA_CONTROL_ENABLE     0x00000001
     76
     77#define TPC_DATA_SEL           0x00008000
     78#define TPC_DIR                0x00004000
     79#define TPC_WAIT_INT           0x00002000
     80#define TPC_GET_INT            0x00000800
     81#define TPC_CODE_SZ_MASK       0x00000700
     82#define TPC_DATA_SZ_MASK       0x00000007
     83
     84#define HOST_CONTROL_TDELAY_EN 0x00040000
     85#define HOST_CONTROL_HW_OC_P   0x00010000
     86#define HOST_CONTROL_RESET_REQ 0x00008000
     87#define HOST_CONTROL_REI       0x00004000
     88#define HOST_CONTROL_LED       0x00000400
     89#define HOST_CONTROL_FAST_CLK  0x00000200
     90#define HOST_CONTROL_RESET     0x00000100
     91#define HOST_CONTROL_POWER_EN  0x00000080
     92#define HOST_CONTROL_CLOCK_EN  0x00000040
     93#define HOST_CONTROL_REO       0x00000008
     94#define HOST_CONTROL_IF_SHIFT  4
     95
     96#define HOST_CONTROL_IF_SERIAL 0x0
     97#define HOST_CONTROL_IF_PAR4   0x1
     98#define HOST_CONTROL_IF_PAR8   0x3
     99
    100#define STATUS_BUSY             0x00080000
    101#define STATUS_MS_DAT7          0x00040000
    102#define STATUS_MS_DAT6          0x00020000
    103#define STATUS_MS_DAT5          0x00010000
    104#define STATUS_MS_DAT4          0x00008000
    105#define STATUS_MS_DAT3          0x00004000
    106#define STATUS_MS_DAT2          0x00002000
    107#define STATUS_MS_DAT1          0x00001000
    108#define STATUS_MS_DAT0          0x00000800
    109#define STATUS_HAS_MEDIA        0x00000400
    110#define STATUS_FIFO_EMPTY       0x00000200
    111#define STATUS_FIFO_FULL        0x00000100
    112#define STATUS_MS_CED           0x00000080
    113#define STATUS_MS_ERR           0x00000040
    114#define STATUS_MS_BRQ           0x00000020
    115#define STATUS_MS_CNK           0x00000001
    116
    117#define INT_STATUS_TPC_ERR      0x00080000
    118#define INT_STATUS_CRC_ERR      0x00040000
    119#define INT_STATUS_TIMER_TO     0x00020000
    120#define INT_STATUS_HSK_TO       0x00010000
    121#define INT_STATUS_ANY_ERR      0x00008000
    122#define INT_STATUS_FIFO_WRDY    0x00000080
    123#define INT_STATUS_FIFO_RRDY    0x00000040
    124#define INT_STATUS_MEDIA_OUT    0x00000010
    125#define INT_STATUS_MEDIA_IN     0x00000008
    126#define INT_STATUS_DMA_BOUNDARY 0x00000004
    127#define INT_STATUS_EOTRAN       0x00000002
    128#define INT_STATUS_EOTPC        0x00000001
    129
    130#define INT_STATUS_ALL          0x000f801f
    131
    132#define PAD_OUTPUT_ENABLE_MS  0x0F3F
    133
    134#define PAD_PU_PD_OFF         0x7FFF0000
    135#define PAD_PU_PD_ON_MS_SOCK0 0x5f8f0000
    136#define PAD_PU_PD_ON_MS_SOCK1 0x0f0f0000
    137
    138#define CLOCK_CONTROL_BY_MMIO 0x00000008
    139#define CLOCK_CONTROL_40MHZ   0x00000001
    140#define CLOCK_CONTROL_50MHZ   0x00000002
    141#define CLOCK_CONTROL_60MHZ   0x00000010
    142#define CLOCK_CONTROL_62_5MHZ 0x00000004
    143#define CLOCK_CONTROL_OFF     0x00000000
    144
    145#define PCI_CTL_CLOCK_DLY_ADDR   0x000000b0
    146
    147enum {
    148	CMD_READY    = 0x01,
    149	FIFO_READY   = 0x02,
    150	REG_DATA     = 0x04,
    151	DMA_DATA     = 0x08
    152};
    153
    154static unsigned int jmb38x_ms_read_data(struct jmb38x_ms_host *host,
    155					unsigned char *buf, unsigned int length)
    156{
    157	unsigned int off = 0;
    158
    159	while (host->io_pos && length) {
    160		buf[off++] = host->io_word[0] & 0xff;
    161		host->io_word[0] >>= 8;
    162		length--;
    163		host->io_pos--;
    164	}
    165
    166	if (!length)
    167		return off;
    168
    169	while (!(STATUS_FIFO_EMPTY & readl(host->addr + STATUS))) {
    170		if (length < 4)
    171			break;
    172		*(unsigned int *)(buf + off) = __raw_readl(host->addr + DATA);
    173		length -= 4;
    174		off += 4;
    175	}
    176
    177	if (length
    178	    && !(STATUS_FIFO_EMPTY & readl(host->addr + STATUS))) {
    179		host->io_word[0] = readl(host->addr + DATA);
    180		for (host->io_pos = 4; host->io_pos; --host->io_pos) {
    181			buf[off++] = host->io_word[0] & 0xff;
    182			host->io_word[0] >>= 8;
    183			length--;
    184			if (!length)
    185				break;
    186		}
    187	}
    188
    189	return off;
    190}
    191
    192static unsigned int jmb38x_ms_read_reg_data(struct jmb38x_ms_host *host,
    193					    unsigned char *buf,
    194					    unsigned int length)
    195{
    196	unsigned int off = 0;
    197
    198	while (host->io_pos > 4 && length) {
    199		buf[off++] = host->io_word[0] & 0xff;
    200		host->io_word[0] >>= 8;
    201		length--;
    202		host->io_pos--;
    203	}
    204
    205	if (!length)
    206		return off;
    207
    208	while (host->io_pos && length) {
    209		buf[off++] = host->io_word[1] & 0xff;
    210		host->io_word[1] >>= 8;
    211		length--;
    212		host->io_pos--;
    213	}
    214
    215	return off;
    216}
    217
    218static unsigned int jmb38x_ms_write_data(struct jmb38x_ms_host *host,
    219					 unsigned char *buf,
    220					 unsigned int length)
    221{
    222	unsigned int off = 0;
    223
    224	if (host->io_pos) {
    225		while (host->io_pos < 4 && length) {
    226			host->io_word[0] |=  buf[off++] << (host->io_pos * 8);
    227			host->io_pos++;
    228			length--;
    229		}
    230	}
    231
    232	if (host->io_pos == 4
    233	    && !(STATUS_FIFO_FULL & readl(host->addr + STATUS))) {
    234		writel(host->io_word[0], host->addr + DATA);
    235		host->io_pos = 0;
    236		host->io_word[0] = 0;
    237	} else if (host->io_pos) {
    238		return off;
    239	}
    240
    241	if (!length)
    242		return off;
    243
    244	while (!(STATUS_FIFO_FULL & readl(host->addr + STATUS))) {
    245		if (length < 4)
    246			break;
    247
    248		__raw_writel(*(unsigned int *)(buf + off),
    249			     host->addr + DATA);
    250		length -= 4;
    251		off += 4;
    252	}
    253
    254	switch (length) {
    255	case 3:
    256		host->io_word[0] |= buf[off + 2] << 16;
    257		host->io_pos++;
    258		fallthrough;
    259	case 2:
    260		host->io_word[0] |= buf[off + 1] << 8;
    261		host->io_pos++;
    262		fallthrough;
    263	case 1:
    264		host->io_word[0] |= buf[off];
    265		host->io_pos++;
    266	}
    267
    268	off += host->io_pos;
    269
    270	return off;
    271}
    272
    273static unsigned int jmb38x_ms_write_reg_data(struct jmb38x_ms_host *host,
    274					     unsigned char *buf,
    275					     unsigned int length)
    276{
    277	unsigned int off = 0;
    278
    279	while (host->io_pos < 4 && length) {
    280		host->io_word[0] &= ~(0xff << (host->io_pos * 8));
    281		host->io_word[0] |=  buf[off++] << (host->io_pos * 8);
    282		host->io_pos++;
    283		length--;
    284	}
    285
    286	if (!length)
    287		return off;
    288
    289	while (host->io_pos < 8 && length) {
    290		host->io_word[1] &= ~(0xff << (host->io_pos * 8));
    291		host->io_word[1] |=  buf[off++] << (host->io_pos * 8);
    292		host->io_pos++;
    293		length--;
    294	}
    295
    296	return off;
    297}
    298
    299static int jmb38x_ms_transfer_data(struct jmb38x_ms_host *host)
    300{
    301	unsigned int length;
    302	unsigned int off;
    303	unsigned int t_size, p_cnt;
    304	unsigned char *buf;
    305	struct page *pg;
    306	unsigned long flags = 0;
    307
    308	if (host->req->long_data) {
    309		length = host->req->sg.length - host->block_pos;
    310		off = host->req->sg.offset + host->block_pos;
    311	} else {
    312		length = host->req->data_len - host->block_pos;
    313		off = 0;
    314	}
    315
    316	while (length) {
    317		unsigned int p_off;
    318
    319		if (host->req->long_data) {
    320			pg = nth_page(sg_page(&host->req->sg),
    321				      off >> PAGE_SHIFT);
    322			p_off = offset_in_page(off);
    323			p_cnt = PAGE_SIZE - p_off;
    324			p_cnt = min(p_cnt, length);
    325
    326			local_irq_save(flags);
    327			buf = kmap_atomic(pg) + p_off;
    328		} else {
    329			buf = host->req->data + host->block_pos;
    330			p_cnt = host->req->data_len - host->block_pos;
    331		}
    332
    333		if (host->req->data_dir == WRITE)
    334			t_size = !(host->cmd_flags & REG_DATA)
    335				 ? jmb38x_ms_write_data(host, buf, p_cnt)
    336				 : jmb38x_ms_write_reg_data(host, buf, p_cnt);
    337		else
    338			t_size = !(host->cmd_flags & REG_DATA)
    339				 ? jmb38x_ms_read_data(host, buf, p_cnt)
    340				 : jmb38x_ms_read_reg_data(host, buf, p_cnt);
    341
    342		if (host->req->long_data) {
    343			kunmap_atomic(buf - p_off);
    344			local_irq_restore(flags);
    345		}
    346
    347		if (!t_size)
    348			break;
    349		host->block_pos += t_size;
    350		length -= t_size;
    351		off += t_size;
    352	}
    353
    354	if (!length && host->req->data_dir == WRITE) {
    355		if (host->cmd_flags & REG_DATA) {
    356			writel(host->io_word[0], host->addr + TPC_P0);
    357			writel(host->io_word[1], host->addr + TPC_P1);
    358		} else if (host->io_pos) {
    359			writel(host->io_word[0], host->addr + DATA);
    360		}
    361	}
    362
    363	return length;
    364}
    365
    366static int jmb38x_ms_issue_cmd(struct memstick_host *msh)
    367{
    368	struct jmb38x_ms_host *host = memstick_priv(msh);
    369	unsigned int data_len, cmd, t_val;
    370
    371	if (!(STATUS_HAS_MEDIA & readl(host->addr + STATUS))) {
    372		dev_dbg(&msh->dev, "no media status\n");
    373		host->req->error = -ETIME;
    374		return host->req->error;
    375	}
    376
    377	dev_dbg(&msh->dev, "control %08x\n", readl(host->addr + HOST_CONTROL));
    378	dev_dbg(&msh->dev, "status %08x\n", readl(host->addr + INT_STATUS));
    379	dev_dbg(&msh->dev, "hstatus %08x\n", readl(host->addr + STATUS));
    380
    381	host->cmd_flags = 0;
    382	host->block_pos = 0;
    383	host->io_pos = 0;
    384	host->io_word[0] = 0;
    385	host->io_word[1] = 0;
    386
    387	cmd = host->req->tpc << 16;
    388	cmd |= TPC_DATA_SEL;
    389
    390	if (host->req->data_dir == READ)
    391		cmd |= TPC_DIR;
    392
    393	if (host->req->need_card_int) {
    394		if (host->ifmode == MEMSTICK_SERIAL)
    395			cmd |= TPC_GET_INT;
    396		else
    397			cmd |= TPC_WAIT_INT;
    398	}
    399
    400	if (!no_dma)
    401		host->cmd_flags |= DMA_DATA;
    402
    403	if (host->req->long_data) {
    404		data_len = host->req->sg.length;
    405	} else {
    406		data_len = host->req->data_len;
    407		host->cmd_flags &= ~DMA_DATA;
    408	}
    409
    410	if (data_len <= 8) {
    411		cmd &= ~(TPC_DATA_SEL | 0xf);
    412		host->cmd_flags |= REG_DATA;
    413		cmd |= data_len & 0xf;
    414		host->cmd_flags &= ~DMA_DATA;
    415	}
    416
    417	if (host->cmd_flags & DMA_DATA) {
    418		if (1 != dma_map_sg(&host->chip->pdev->dev, &host->req->sg, 1,
    419				    host->req->data_dir == READ
    420				    ? DMA_FROM_DEVICE
    421				    : DMA_TO_DEVICE)) {
    422			host->req->error = -ENOMEM;
    423			return host->req->error;
    424		}
    425		data_len = sg_dma_len(&host->req->sg);
    426		writel(sg_dma_address(&host->req->sg),
    427		       host->addr + DMA_ADDRESS);
    428		writel(((1 << 16) & BLOCK_COUNT_MASK)
    429		       | (data_len & BLOCK_SIZE_MASK),
    430		       host->addr + BLOCK);
    431		writel(DMA_CONTROL_ENABLE, host->addr + DMA_CONTROL);
    432	} else if (!(host->cmd_flags & REG_DATA)) {
    433		writel(((1 << 16) & BLOCK_COUNT_MASK)
    434		       | (data_len & BLOCK_SIZE_MASK),
    435		       host->addr + BLOCK);
    436		t_val = readl(host->addr + INT_STATUS_ENABLE);
    437		t_val |= host->req->data_dir == READ
    438			 ? INT_STATUS_FIFO_RRDY
    439			 : INT_STATUS_FIFO_WRDY;
    440
    441		writel(t_val, host->addr + INT_STATUS_ENABLE);
    442		writel(t_val, host->addr + INT_SIGNAL_ENABLE);
    443	} else {
    444		cmd &= ~(TPC_DATA_SEL | 0xf);
    445		host->cmd_flags |= REG_DATA;
    446		cmd |= data_len & 0xf;
    447
    448		if (host->req->data_dir == WRITE) {
    449			jmb38x_ms_transfer_data(host);
    450			writel(host->io_word[0], host->addr + TPC_P0);
    451			writel(host->io_word[1], host->addr + TPC_P1);
    452		}
    453	}
    454
    455	mod_timer(&host->timer, jiffies + host->timeout_jiffies);
    456	writel(HOST_CONTROL_LED | readl(host->addr + HOST_CONTROL),
    457	       host->addr + HOST_CONTROL);
    458	host->req->error = 0;
    459
    460	writel(cmd, host->addr + TPC);
    461	dev_dbg(&msh->dev, "executing TPC %08x, len %x\n", cmd, data_len);
    462
    463	return 0;
    464}
    465
    466static void jmb38x_ms_complete_cmd(struct memstick_host *msh, int last)
    467{
    468	struct jmb38x_ms_host *host = memstick_priv(msh);
    469	unsigned int t_val = 0;
    470	int rc;
    471
    472	del_timer(&host->timer);
    473
    474	dev_dbg(&msh->dev, "c control %08x\n",
    475		readl(host->addr + HOST_CONTROL));
    476	dev_dbg(&msh->dev, "c status %08x\n",
    477		readl(host->addr + INT_STATUS));
    478	dev_dbg(&msh->dev, "c hstatus %08x\n", readl(host->addr + STATUS));
    479
    480	host->req->int_reg = readl(host->addr + STATUS) & 0xff;
    481
    482	writel(0, host->addr + BLOCK);
    483	writel(0, host->addr + DMA_CONTROL);
    484
    485	if (host->cmd_flags & DMA_DATA) {
    486		dma_unmap_sg(&host->chip->pdev->dev, &host->req->sg, 1,
    487			     host->req->data_dir == READ
    488			     ? DMA_FROM_DEVICE : DMA_TO_DEVICE);
    489	} else {
    490		t_val = readl(host->addr + INT_STATUS_ENABLE);
    491		if (host->req->data_dir == READ)
    492			t_val &= ~INT_STATUS_FIFO_RRDY;
    493		else
    494			t_val &= ~INT_STATUS_FIFO_WRDY;
    495
    496		writel(t_val, host->addr + INT_STATUS_ENABLE);
    497		writel(t_val, host->addr + INT_SIGNAL_ENABLE);
    498	}
    499
    500	writel((~HOST_CONTROL_LED) & readl(host->addr + HOST_CONTROL),
    501	       host->addr + HOST_CONTROL);
    502
    503	if (!last) {
    504		do {
    505			rc = memstick_next_req(msh, &host->req);
    506		} while (!rc && jmb38x_ms_issue_cmd(msh));
    507	} else {
    508		do {
    509			rc = memstick_next_req(msh, &host->req);
    510			if (!rc)
    511				host->req->error = -ETIME;
    512		} while (!rc);
    513	}
    514}
    515
    516static irqreturn_t jmb38x_ms_isr(int irq, void *dev_id)
    517{
    518	struct memstick_host *msh = dev_id;
    519	struct jmb38x_ms_host *host = memstick_priv(msh);
    520	unsigned int irq_status;
    521
    522	spin_lock(&host->lock);
    523	irq_status = readl(host->addr + INT_STATUS);
    524	dev_dbg(&host->chip->pdev->dev, "irq_status = %08x\n", irq_status);
    525	if (irq_status == 0 || irq_status == (~0)) {
    526		spin_unlock(&host->lock);
    527		return IRQ_NONE;
    528	}
    529
    530	if (host->req) {
    531		if (irq_status & INT_STATUS_ANY_ERR) {
    532			if (irq_status & INT_STATUS_CRC_ERR)
    533				host->req->error = -EILSEQ;
    534			else if (irq_status & INT_STATUS_TPC_ERR) {
    535				dev_dbg(&host->chip->pdev->dev, "TPC_ERR\n");
    536				jmb38x_ms_complete_cmd(msh, 0);
    537			} else
    538				host->req->error = -ETIME;
    539		} else {
    540			if (host->cmd_flags & DMA_DATA) {
    541				if (irq_status & INT_STATUS_EOTRAN)
    542					host->cmd_flags |= FIFO_READY;
    543			} else {
    544				if (irq_status & (INT_STATUS_FIFO_RRDY
    545						  | INT_STATUS_FIFO_WRDY))
    546					jmb38x_ms_transfer_data(host);
    547
    548				if (irq_status & INT_STATUS_EOTRAN) {
    549					jmb38x_ms_transfer_data(host);
    550					host->cmd_flags |= FIFO_READY;
    551				}
    552			}
    553
    554			if (irq_status & INT_STATUS_EOTPC) {
    555				host->cmd_flags |= CMD_READY;
    556				if (host->cmd_flags & REG_DATA) {
    557					if (host->req->data_dir == READ) {
    558						host->io_word[0]
    559							= readl(host->addr
    560								+ TPC_P0);
    561						host->io_word[1]
    562							= readl(host->addr
    563								+ TPC_P1);
    564						host->io_pos = 8;
    565
    566						jmb38x_ms_transfer_data(host);
    567					}
    568					host->cmd_flags |= FIFO_READY;
    569				}
    570			}
    571		}
    572	}
    573
    574	if (irq_status & (INT_STATUS_MEDIA_IN | INT_STATUS_MEDIA_OUT)) {
    575		dev_dbg(&host->chip->pdev->dev, "media changed\n");
    576		memstick_detect_change(msh);
    577	}
    578
    579	writel(irq_status, host->addr + INT_STATUS);
    580
    581	if (host->req
    582	    && (((host->cmd_flags & CMD_READY)
    583		 && (host->cmd_flags & FIFO_READY))
    584		|| host->req->error))
    585		jmb38x_ms_complete_cmd(msh, 0);
    586
    587	spin_unlock(&host->lock);
    588	return IRQ_HANDLED;
    589}
    590
    591static void jmb38x_ms_abort(struct timer_list *t)
    592{
    593	struct jmb38x_ms_host *host = from_timer(host, t, timer);
    594	struct memstick_host *msh = host->msh;
    595	unsigned long flags;
    596
    597	dev_dbg(&host->chip->pdev->dev, "abort\n");
    598	spin_lock_irqsave(&host->lock, flags);
    599	if (host->req) {
    600		host->req->error = -ETIME;
    601		jmb38x_ms_complete_cmd(msh, 0);
    602	}
    603	spin_unlock_irqrestore(&host->lock, flags);
    604}
    605
    606static void jmb38x_ms_req_tasklet(unsigned long data)
    607{
    608	struct memstick_host *msh = (struct memstick_host *)data;
    609	struct jmb38x_ms_host *host = memstick_priv(msh);
    610	unsigned long flags;
    611	int rc;
    612
    613	spin_lock_irqsave(&host->lock, flags);
    614	if (!host->req) {
    615		do {
    616			rc = memstick_next_req(msh, &host->req);
    617			dev_dbg(&host->chip->pdev->dev, "tasklet req %d\n", rc);
    618		} while (!rc && jmb38x_ms_issue_cmd(msh));
    619	}
    620	spin_unlock_irqrestore(&host->lock, flags);
    621}
    622
    623static void jmb38x_ms_dummy_submit(struct memstick_host *msh)
    624{
    625	return;
    626}
    627
    628static void jmb38x_ms_submit_req(struct memstick_host *msh)
    629{
    630	struct jmb38x_ms_host *host = memstick_priv(msh);
    631
    632	tasklet_schedule(&host->notify);
    633}
    634
    635static int jmb38x_ms_reset(struct jmb38x_ms_host *host)
    636{
    637	int cnt;
    638
    639	writel(HOST_CONTROL_RESET_REQ | HOST_CONTROL_CLOCK_EN
    640	       | readl(host->addr + HOST_CONTROL),
    641	       host->addr + HOST_CONTROL);
    642
    643	for (cnt = 0; cnt < 20; ++cnt) {
    644		if (!(HOST_CONTROL_RESET_REQ
    645		      & readl(host->addr + HOST_CONTROL)))
    646			goto reset_next;
    647
    648		ndelay(20);
    649	}
    650	dev_dbg(&host->chip->pdev->dev, "reset_req timeout\n");
    651
    652reset_next:
    653	writel(HOST_CONTROL_RESET | HOST_CONTROL_CLOCK_EN
    654	       | readl(host->addr + HOST_CONTROL),
    655	       host->addr + HOST_CONTROL);
    656
    657	for (cnt = 0; cnt < 20; ++cnt) {
    658		if (!(HOST_CONTROL_RESET
    659		      & readl(host->addr + HOST_CONTROL)))
    660			goto reset_ok;
    661
    662		ndelay(20);
    663	}
    664	dev_dbg(&host->chip->pdev->dev, "reset timeout\n");
    665	return -EIO;
    666
    667reset_ok:
    668	writel(INT_STATUS_ALL, host->addr + INT_SIGNAL_ENABLE);
    669	writel(INT_STATUS_ALL, host->addr + INT_STATUS_ENABLE);
    670	return 0;
    671}
    672
    673static int jmb38x_ms_set_param(struct memstick_host *msh,
    674			       enum memstick_param param,
    675			       int value)
    676{
    677	struct jmb38x_ms_host *host = memstick_priv(msh);
    678	unsigned int host_ctl = readl(host->addr + HOST_CONTROL);
    679	unsigned int clock_ctl = CLOCK_CONTROL_BY_MMIO, clock_delay = 0;
    680	int rc = 0;
    681
    682	switch (param) {
    683	case MEMSTICK_POWER:
    684		if (value == MEMSTICK_POWER_ON) {
    685			rc = jmb38x_ms_reset(host);
    686			if (rc)
    687				return rc;
    688
    689			host_ctl = 7;
    690			host_ctl |= HOST_CONTROL_POWER_EN
    691				 | HOST_CONTROL_CLOCK_EN;
    692			writel(host_ctl, host->addr + HOST_CONTROL);
    693
    694			writel(host->id ? PAD_PU_PD_ON_MS_SOCK1
    695					: PAD_PU_PD_ON_MS_SOCK0,
    696			       host->addr + PAD_PU_PD);
    697
    698			writel(PAD_OUTPUT_ENABLE_MS,
    699			       host->addr + PAD_OUTPUT_ENABLE);
    700
    701			msleep(10);
    702			dev_dbg(&host->chip->pdev->dev, "power on\n");
    703		} else if (value == MEMSTICK_POWER_OFF) {
    704			host_ctl &= ~(HOST_CONTROL_POWER_EN
    705				      | HOST_CONTROL_CLOCK_EN);
    706			writel(host_ctl, host->addr +  HOST_CONTROL);
    707			writel(0, host->addr + PAD_OUTPUT_ENABLE);
    708			writel(PAD_PU_PD_OFF, host->addr + PAD_PU_PD);
    709			dev_dbg(&host->chip->pdev->dev, "power off\n");
    710		} else
    711			return -EINVAL;
    712		break;
    713	case MEMSTICK_INTERFACE:
    714		dev_dbg(&host->chip->pdev->dev,
    715			"Set Host Interface Mode to %d\n", value);
    716		host_ctl &= ~(HOST_CONTROL_FAST_CLK | HOST_CONTROL_REI |
    717			      HOST_CONTROL_REO);
    718		host_ctl |= HOST_CONTROL_TDELAY_EN | HOST_CONTROL_HW_OC_P;
    719		host_ctl &= ~(3 << HOST_CONTROL_IF_SHIFT);
    720
    721		if (value == MEMSTICK_SERIAL) {
    722			host_ctl |= HOST_CONTROL_IF_SERIAL
    723				    << HOST_CONTROL_IF_SHIFT;
    724			host_ctl |= HOST_CONTROL_REI;
    725			clock_ctl |= CLOCK_CONTROL_40MHZ;
    726			clock_delay = 0;
    727		} else if (value == MEMSTICK_PAR4) {
    728			host_ctl |= HOST_CONTROL_FAST_CLK;
    729			host_ctl |= HOST_CONTROL_IF_PAR4
    730				    << HOST_CONTROL_IF_SHIFT;
    731			host_ctl |= HOST_CONTROL_REO;
    732			clock_ctl |= CLOCK_CONTROL_40MHZ;
    733			clock_delay = 4;
    734		} else if (value == MEMSTICK_PAR8) {
    735			host_ctl |= HOST_CONTROL_FAST_CLK;
    736			host_ctl |= HOST_CONTROL_IF_PAR8
    737				    << HOST_CONTROL_IF_SHIFT;
    738			clock_ctl |= CLOCK_CONTROL_50MHZ;
    739			clock_delay = 0;
    740		} else
    741			return -EINVAL;
    742
    743		writel(host_ctl, host->addr + HOST_CONTROL);
    744		writel(CLOCK_CONTROL_OFF, host->addr + CLOCK_CONTROL);
    745		writel(clock_ctl, host->addr + CLOCK_CONTROL);
    746		pci_write_config_byte(host->chip->pdev,
    747				      PCI_CTL_CLOCK_DLY_ADDR + 1,
    748				      clock_delay);
    749		host->ifmode = value;
    750		break;
    751	}
    752	return 0;
    753}
    754
    755#define PCI_PMOS0_CONTROL		0xae
    756#define  PMOS0_ENABLE			0x01
    757#define  PMOS0_OVERCURRENT_LEVEL_2_4V	0x06
    758#define  PMOS0_EN_OVERCURRENT_DEBOUNCE	0x40
    759#define  PMOS0_SW_LED_POLARITY_ENABLE	0x80
    760#define  PMOS0_ACTIVE_BITS (PMOS0_ENABLE | PMOS0_EN_OVERCURRENT_DEBOUNCE | \
    761			    PMOS0_OVERCURRENT_LEVEL_2_4V)
    762#define PCI_PMOS1_CONTROL		0xbd
    763#define  PMOS1_ACTIVE_BITS		0x4a
    764#define PCI_CLOCK_CTL			0xb9
    765
    766static int jmb38x_ms_pmos(struct pci_dev *pdev, int flag)
    767{
    768	unsigned char val;
    769
    770	pci_read_config_byte(pdev, PCI_PMOS0_CONTROL, &val);
    771	if (flag)
    772		val |= PMOS0_ACTIVE_BITS;
    773	else
    774		val &= ~PMOS0_ACTIVE_BITS;
    775	pci_write_config_byte(pdev, PCI_PMOS0_CONTROL, val);
    776	dev_dbg(&pdev->dev, "JMB38x: set PMOS0 val 0x%x\n", val);
    777
    778	if (pci_resource_flags(pdev, 1)) {
    779		pci_read_config_byte(pdev, PCI_PMOS1_CONTROL, &val);
    780		if (flag)
    781			val |= PMOS1_ACTIVE_BITS;
    782		else
    783			val &= ~PMOS1_ACTIVE_BITS;
    784		pci_write_config_byte(pdev, PCI_PMOS1_CONTROL, val);
    785		dev_dbg(&pdev->dev, "JMB38x: set PMOS1 val 0x%x\n", val);
    786	}
    787
    788	pci_read_config_byte(pdev, PCI_CLOCK_CTL, &val);
    789	pci_write_config_byte(pdev, PCI_CLOCK_CTL, val & ~0x0f);
    790	pci_write_config_byte(pdev, PCI_CLOCK_CTL, val | 0x01);
    791	dev_dbg(&pdev->dev, "Clock Control by PCI config is disabled!\n");
    792
    793        return 0;
    794}
    795
    796static int __maybe_unused jmb38x_ms_suspend(struct device *dev)
    797{
    798	struct jmb38x_ms *jm = dev_get_drvdata(dev);
    799
    800	int cnt;
    801
    802	for (cnt = 0; cnt < jm->host_cnt; ++cnt) {
    803		if (!jm->hosts[cnt])
    804			break;
    805		memstick_suspend_host(jm->hosts[cnt]);
    806	}
    807
    808	device_wakeup_disable(dev);
    809
    810	return 0;
    811}
    812
    813static int __maybe_unused jmb38x_ms_resume(struct device *dev)
    814{
    815	struct jmb38x_ms *jm = dev_get_drvdata(dev);
    816	int rc;
    817
    818	jmb38x_ms_pmos(to_pci_dev(dev), 1);
    819
    820	for (rc = 0; rc < jm->host_cnt; ++rc) {
    821		if (!jm->hosts[rc])
    822			break;
    823		memstick_resume_host(jm->hosts[rc]);
    824		memstick_detect_change(jm->hosts[rc]);
    825	}
    826
    827	return 0;
    828}
    829
    830static int jmb38x_ms_count_slots(struct pci_dev *pdev)
    831{
    832	int cnt, rc = 0;
    833
    834	for (cnt = 0; cnt < PCI_STD_NUM_BARS; ++cnt) {
    835		if (!(IORESOURCE_MEM & pci_resource_flags(pdev, cnt)))
    836			break;
    837
    838		if (256 != pci_resource_len(pdev, cnt))
    839			break;
    840
    841		++rc;
    842	}
    843	return rc;
    844}
    845
    846static struct memstick_host *jmb38x_ms_alloc_host(struct jmb38x_ms *jm, int cnt)
    847{
    848	struct memstick_host *msh;
    849	struct jmb38x_ms_host *host;
    850
    851	msh = memstick_alloc_host(sizeof(struct jmb38x_ms_host),
    852				  &jm->pdev->dev);
    853	if (!msh)
    854		return NULL;
    855
    856	host = memstick_priv(msh);
    857	host->msh = msh;
    858	host->chip = jm;
    859	host->addr = ioremap(pci_resource_start(jm->pdev, cnt),
    860			     pci_resource_len(jm->pdev, cnt));
    861	if (!host->addr)
    862		goto err_out_free;
    863
    864	spin_lock_init(&host->lock);
    865	host->id = cnt;
    866	snprintf(host->host_id, sizeof(host->host_id), DRIVER_NAME ":slot%d",
    867		 host->id);
    868	host->irq = jm->pdev->irq;
    869	host->timeout_jiffies = msecs_to_jiffies(1000);
    870
    871	tasklet_init(&host->notify, jmb38x_ms_req_tasklet, (unsigned long)msh);
    872	msh->request = jmb38x_ms_submit_req;
    873	msh->set_param = jmb38x_ms_set_param;
    874
    875	msh->caps = MEMSTICK_CAP_PAR4 | MEMSTICK_CAP_PAR8;
    876
    877	timer_setup(&host->timer, jmb38x_ms_abort, 0);
    878
    879	if (!request_irq(host->irq, jmb38x_ms_isr, IRQF_SHARED, host->host_id,
    880			 msh))
    881		return msh;
    882
    883	iounmap(host->addr);
    884err_out_free:
    885	memstick_free_host(msh);
    886	return NULL;
    887}
    888
    889static void jmb38x_ms_free_host(struct memstick_host *msh)
    890{
    891	struct jmb38x_ms_host *host = memstick_priv(msh);
    892
    893	free_irq(host->irq, msh);
    894	iounmap(host->addr);
    895	memstick_free_host(msh);
    896}
    897
    898static int jmb38x_ms_probe(struct pci_dev *pdev,
    899			   const struct pci_device_id *dev_id)
    900{
    901	struct jmb38x_ms *jm;
    902	int pci_dev_busy = 0;
    903	int rc, cnt;
    904
    905	rc = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
    906	if (rc)
    907		return rc;
    908
    909	rc = pci_enable_device(pdev);
    910	if (rc)
    911		return rc;
    912
    913	pci_set_master(pdev);
    914
    915	rc = pci_request_regions(pdev, DRIVER_NAME);
    916	if (rc) {
    917		pci_dev_busy = 1;
    918		goto err_out;
    919	}
    920
    921	jmb38x_ms_pmos(pdev, 1);
    922
    923	cnt = jmb38x_ms_count_slots(pdev);
    924	if (!cnt) {
    925		rc = -ENODEV;
    926		pci_dev_busy = 1;
    927		goto err_out_int;
    928	}
    929
    930	jm = kzalloc(struct_size(jm, hosts, cnt), GFP_KERNEL);
    931	if (!jm) {
    932		rc = -ENOMEM;
    933		goto err_out_int;
    934	}
    935
    936	jm->pdev = pdev;
    937	jm->host_cnt = cnt;
    938	pci_set_drvdata(pdev, jm);
    939
    940	for (cnt = 0; cnt < jm->host_cnt; ++cnt) {
    941		jm->hosts[cnt] = jmb38x_ms_alloc_host(jm, cnt);
    942		if (!jm->hosts[cnt])
    943			break;
    944
    945		rc = memstick_add_host(jm->hosts[cnt]);
    946
    947		if (rc) {
    948			jmb38x_ms_free_host(jm->hosts[cnt]);
    949			jm->hosts[cnt] = NULL;
    950			break;
    951		}
    952	}
    953
    954	if (cnt)
    955		return 0;
    956
    957	rc = -ENODEV;
    958
    959	pci_set_drvdata(pdev, NULL);
    960	kfree(jm);
    961err_out_int:
    962	pci_release_regions(pdev);
    963err_out:
    964	if (!pci_dev_busy)
    965		pci_disable_device(pdev);
    966	return rc;
    967}
    968
    969static void jmb38x_ms_remove(struct pci_dev *dev)
    970{
    971	struct jmb38x_ms *jm = pci_get_drvdata(dev);
    972	struct jmb38x_ms_host *host;
    973	int cnt;
    974	unsigned long flags;
    975
    976	for (cnt = 0; cnt < jm->host_cnt; ++cnt) {
    977		if (!jm->hosts[cnt])
    978			break;
    979
    980		host = memstick_priv(jm->hosts[cnt]);
    981
    982		jm->hosts[cnt]->request = jmb38x_ms_dummy_submit;
    983		tasklet_kill(&host->notify);
    984		writel(0, host->addr + INT_SIGNAL_ENABLE);
    985		writel(0, host->addr + INT_STATUS_ENABLE);
    986		dev_dbg(&jm->pdev->dev, "interrupts off\n");
    987		spin_lock_irqsave(&host->lock, flags);
    988		if (host->req) {
    989			host->req->error = -ETIME;
    990			jmb38x_ms_complete_cmd(jm->hosts[cnt], 1);
    991		}
    992		spin_unlock_irqrestore(&host->lock, flags);
    993
    994		memstick_remove_host(jm->hosts[cnt]);
    995		dev_dbg(&jm->pdev->dev, "host removed\n");
    996
    997		jmb38x_ms_free_host(jm->hosts[cnt]);
    998	}
    999
   1000	jmb38x_ms_pmos(dev, 0);
   1001
   1002	pci_set_drvdata(dev, NULL);
   1003	pci_release_regions(dev);
   1004	pci_disable_device(dev);
   1005	kfree(jm);
   1006}
   1007
   1008static struct pci_device_id jmb38x_ms_id_tbl [] = {
   1009	{ PCI_VDEVICE(JMICRON, PCI_DEVICE_ID_JMICRON_JMB38X_MS) },
   1010	{ PCI_VDEVICE(JMICRON, PCI_DEVICE_ID_JMICRON_JMB385_MS) },
   1011	{ PCI_VDEVICE(JMICRON, PCI_DEVICE_ID_JMICRON_JMB390_MS) },
   1012	{ }
   1013};
   1014
   1015static SIMPLE_DEV_PM_OPS(jmb38x_ms_pm_ops, jmb38x_ms_suspend, jmb38x_ms_resume);
   1016
   1017static struct pci_driver jmb38x_ms_driver = {
   1018	.name = DRIVER_NAME,
   1019	.id_table = jmb38x_ms_id_tbl,
   1020	.probe = jmb38x_ms_probe,
   1021	.remove = jmb38x_ms_remove,
   1022	.driver.pm = &jmb38x_ms_pm_ops,
   1023};
   1024
   1025module_pci_driver(jmb38x_ms_driver);
   1026
   1027MODULE_AUTHOR("Alex Dubov");
   1028MODULE_DESCRIPTION("JMicron jmb38x MemoryStick driver");
   1029MODULE_LICENSE("GPL");
   1030MODULE_DEVICE_TABLE(pci, jmb38x_ms_id_tbl);