summaryrefslogtreecommitdiffstats
path: root/driver/hdm-dim2
diff options
context:
space:
mode:
Diffstat (limited to 'driver/hdm-dim2')
-rw-r--r--driver/hdm-dim2/dim2_hal.c117
-rw-r--r--driver/hdm-dim2/dim2_hal.h9
-rw-r--r--driver/hdm-dim2/dim2_hdm.c105
-rw-r--r--driver/hdm-dim2/dim2_reg.h3
-rw-r--r--driver/hdm-dim2/dim2_sysfs.c2
5 files changed, 158 insertions, 78 deletions
diff --git a/driver/hdm-dim2/dim2_hal.c b/driver/hdm-dim2/dim2_hal.c
index 6f2b6e4..0b9816c 100644
--- a/driver/hdm-dim2/dim2_hal.c
+++ b/driver/hdm-dim2/dim2_hal.c
@@ -2,7 +2,7 @@
* dim2_hal.c - DIM2 HAL implementation
* (MediaLB, Device Interface Macro IP, OS62420)
*
- * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ * Copyright (C) 2015-2016, Microchip Technology Germany II GmbH & Co. KG
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -49,6 +49,8 @@
#define DBR_SIZE (16 * 1024) /* specified by IP */
#define DBR_BLOCK_SIZE (DBR_SIZE / 32 / DBR_MAP_SIZE)
+#define ROUND_UP_TO(x, d) (((x) + (d) - 1) / (d) * (d))
+
/* -------------------------------------------------------------------------- */
/* generic helper functions and macros */
@@ -66,10 +68,19 @@ static inline bool dim_on_error(u8 error_id, const char *error_message)
/* -------------------------------------------------------------------------- */
/* types and local variables */
+struct async_tx_dbr {
+ u8 ch_addr;
+ u16 rpc;
+ u16 wpc;
+ u16 rest_size;
+ u16 sz_queue[CDT0_RPC_MASK + 1];
+};
+
struct lld_global_vars_t {
bool dim_is_initialized;
bool mcm_is_initialized;
struct dim2_regs __iomem *dim2; /* DIM2 core base address */
+ struct async_tx_dbr atx_dbr;
u32 fcnt;
u32 dbr_map[DBR_MAP_SIZE];
};
@@ -251,6 +262,13 @@ static void dim2_configure_cdt(u8 ch_addr, u16 dbr_address, u16 hw_buffer_size,
dim2_write_ctr(CDT + ch_addr, cdt);
}
+static u16 dim2_rpc(u8 ch_addr)
+{
+ u32 cdt0 = dim2_read_ctr(CDT + ch_addr, 0);
+
+ return (cdt0 >> CDT0_RPC_SHIFT) & CDT0_RPC_MASK;
+}
+
static void dim2_clear_cdt(u8 ch_addr)
{
u32 cdt[4] = { 0, 0, 0, 0 };
@@ -361,6 +379,49 @@ static void dim2_clear_channel(u8 ch_addr)
}
/* -------------------------------------------------------------------------- */
+/* trace async tx dbr fill state */
+
+static inline u16 norm_pc(u16 pc)
+{
+ return pc & CDT0_RPC_MASK;
+}
+
+static void dbrcnt_init(u8 ch_addr, u16 dbr_size)
+{
+ g.atx_dbr.rest_size = dbr_size;
+ g.atx_dbr.rpc = dim2_rpc(ch_addr);
+ g.atx_dbr.wpc = g.atx_dbr.rpc;
+}
+
+static void dbrcnt_enq(int buf_sz)
+{
+ g.atx_dbr.rest_size -= buf_sz;
+ g.atx_dbr.sz_queue[norm_pc(g.atx_dbr.wpc)] = buf_sz;
+ g.atx_dbr.wpc++;
+}
+
+u16 dim_dbr_space(struct dim_channel *ch)
+{
+ u16 cur_rpc;
+ struct async_tx_dbr *dbr = &g.atx_dbr;
+
+ if (ch->addr != dbr->ch_addr)
+ return 0xFFFF;
+
+ cur_rpc = dim2_rpc(ch->addr);
+
+ while (norm_pc(dbr->rpc) != cur_rpc) {
+ dbr->rest_size += dbr->sz_queue[norm_pc(dbr->rpc)];
+ dbr->rpc++;
+ }
+
+ if ((u16)(dbr->wpc - dbr->rpc) >= CDT0_RPC_MASK)
+ return 0;
+
+ return dbr->rest_size;
+}
+
+/* -------------------------------------------------------------------------- */
/* channel state helpers */
static void state_init(struct int_ch_state *state)
@@ -518,20 +579,17 @@ static inline bool service_channel(u8 ch_addr, u8 idx)
{
u8 const shift = idx * 16;
u32 const adt1 = dim2_read_ctr(ADT + ch_addr, 1);
+ u32 mask[4] = { 0, 0, 0, 0 };
+ u32 adt_w[4] = { 0, 0, 0, 0 };
if (((adt1 >> (ADT1_DNE_BIT + shift)) & 1) == 0)
return false;
- {
- u32 mask[4] = { 0, 0, 0, 0 };
- u32 adt_w[4] = { 0, 0, 0, 0 };
-
- mask[1] =
- bit_mask(ADT1_DNE_BIT + shift) |
- bit_mask(ADT1_ERR_BIT + shift) |
- bit_mask(ADT1_RDY_BIT + shift);
- dim2_write_ctr_mask(ADT + ch_addr, mask, adt_w);
- }
+ mask[1] =
+ bit_mask(ADT1_DNE_BIT + shift) |
+ bit_mask(ADT1_ERR_BIT + shift) |
+ bit_mask(ADT1_RDY_BIT + shift);
+ dim2_write_ctr_mask(ADT + ch_addr, mask, adt_w);
/* clear channel status bit */
dimcb_io_write(&g.dim2->ACSR0, bit_mask(ch_addr));
@@ -615,6 +673,9 @@ static bool channel_start(struct dim_channel *ch, u32 buf_addr, u16 buf_size)
++state->level;
+ if (ch->addr == g.atx_dbr.ch_addr)
+ dbrcnt_enq(buf_size);
+
if (ch->packet_length || ch->bytes_per_frame)
dim2_start_isoc_sync(ch->addr, state->idx1, buf_addr, buf_size);
else
@@ -653,7 +714,8 @@ static bool channel_detach_buffers(struct dim_channel *ch, u16 buffers_number)
/* -------------------------------------------------------------------------- */
/* API */
-u8 dim_startup(struct dim2_regs __iomem *dim_base_address, u32 mlb_clock, u32 fcnt)
+u8 dim_startup(struct dim2_regs __iomem *dim_base_address, u32 mlb_clock,
+ u32 fcnt)
{
g.dim_is_initialized = false;
@@ -700,7 +762,7 @@ static u8 init_ctrl_async(struct dim_channel *ch, u8 type, u8 is_tx,
if (!check_channel_address(ch_address))
return DIM_INIT_ERR_CHANNEL_ADDRESS;
- ch->dbr_size = hw_buffer_size;
+ ch->dbr_size = ROUND_UP_TO(hw_buffer_size, DBR_BLOCK_SIZE);
ch->dbr_addr = alloc_dbr(ch->dbr_size);
if (ch->dbr_addr >= DBR_SIZE)
return DIM_INIT_ERR_OUT_OF_MEMORY;
@@ -713,6 +775,12 @@ static u8 init_ctrl_async(struct dim_channel *ch, u8 type, u8 is_tx,
return DIM_NO_ERROR;
}
+void dim_service_mlb_int_irq(void)
+{
+ dimcb_io_write(&g.dim2->MS0, 0);
+ dimcb_io_write(&g.dim2->MS1, 0);
+}
+
u16 dim_norm_ctrl_async_buffer_size(u16 buf_size)
{
return norm_ctrl_async_buffer_size(buf_size);
@@ -756,8 +824,16 @@ u8 dim_init_control(struct dim_channel *ch, u8 is_tx, u16 ch_address,
u8 dim_init_async(struct dim_channel *ch, u8 is_tx, u16 ch_address,
u16 max_buffer_size)
{
- return init_ctrl_async(ch, CAT_CT_VAL_ASYNC, is_tx, ch_address,
- max_buffer_size);
+ u8 ret = init_ctrl_async(ch, CAT_CT_VAL_ASYNC, is_tx, ch_address,
+ max_buffer_size);
+
+ if (is_tx && !g.atx_dbr.ch_addr) {
+ g.atx_dbr.ch_addr = ch->addr;
+ dbrcnt_init(ch->addr, ch->dbr_size);
+ dimcb_io_write(&g.dim2->MIEN, bit_mask(20));
+ }
+
+ return ret;
}
u8 dim_init_isoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
@@ -818,6 +894,11 @@ u8 dim_destroy_channel(struct dim_channel *ch)
if (!g.dim_is_initialized || !ch)
return DIM_ERR_DRIVER_NOT_INITIALIZED;
+ if (ch->addr == g.atx_dbr.ch_addr) {
+ dimcb_io_write(&g.dim2->MIEN, 0);
+ g.atx_dbr.ch_addr = 0;
+ }
+
dim2_clear_channel(ch->addr);
if (ch->dbr_addr < DBR_SIZE)
free_dbr(ch->dbr_addr, ch->dbr_size);
@@ -826,7 +907,7 @@ u8 dim_destroy_channel(struct dim_channel *ch)
return DIM_NO_ERROR;
}
-void dim_service_irq(struct dim_channel *const *channels)
+void dim_service_ahb_int_irq(struct dim_channel *const *channels)
{
bool state_changed;
@@ -858,10 +939,6 @@ void dim_service_irq(struct dim_channel *const *channels)
++ch;
}
} while (state_changed);
-
- /* clear pending Interrupts */
- dimcb_io_write(&g.dim2->MS0, 0);
- dimcb_io_write(&g.dim2->MS1, 0);
}
u8 dim_service_channel(struct dim_channel *ch)
diff --git a/driver/hdm-dim2/dim2_hal.h b/driver/hdm-dim2/dim2_hal.h
index 453634a..6df6ea5 100644
--- a/driver/hdm-dim2/dim2_hal.h
+++ b/driver/hdm-dim2/dim2_hal.h
@@ -60,7 +60,8 @@ struct dim_channel {
u16 done_sw_buffers_number; /*< Done software buffers number. */
};
-u8 dim_startup(struct dim2_regs __iomem *dim_base_address, u32 mlb_clock, u32 fcnt);
+u8 dim_startup(struct dim2_regs __iomem *dim_base_address, u32 mlb_clock,
+ u32 fcnt);
void dim_shutdown(void);
@@ -86,13 +87,17 @@ u8 dim_init_sync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
u8 dim_destroy_channel(struct dim_channel *ch);
-void dim_service_irq(struct dim_channel *const *channels);
+void dim_service_mlb_int_irq(void);
+
+void dim_service_ahb_int_irq(struct dim_channel *const *channels);
u8 dim_service_channel(struct dim_channel *ch);
struct dim_ch_state_t *dim_get_channel_state(struct dim_channel *ch,
struct dim_ch_state_t *state_ptr);
+u16 dim_dbr_space(struct dim_channel *ch);
+
bool dim_enqueue_buffer(struct dim_channel *ch, u32 buffer_addr,
u16 buffer_size);
diff --git a/driver/hdm-dim2/dim2_hdm.c b/driver/hdm-dim2/dim2_hdm.c
index 87039d9..35aee9f 100644
--- a/driver/hdm-dim2/dim2_hdm.c
+++ b/driver/hdm-dim2/dim2_hdm.c
@@ -1,7 +1,7 @@
/*
* dim2_hdm.c - MediaLB DIM2 Hardware Dependent Module
*
- * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG
+ * Copyright (C) 2015-2016, Microchip Technology Germany II GmbH & Co. KG
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -55,17 +55,6 @@ static u8 fcnt = 4; /* (1 << fcnt) frames per subbuffer */
module_param(fcnt, byte, 0);
MODULE_PARM_DESC(fcnt, "Num of frames per sub-buffer for sync channels as a power of 2");
-/*
- * #############################################################################
- *
- * The define below activates an utility function used by HAL-simu
- * for calling DIM interrupt handler.
- * It is used only for TEST PURPOSE and shall be commented before release.
- *
- * #############################################################################
- */
-/* #define ENABLE_HDM_TEST */
-
static DEFINE_SPINLOCK(dim_lock);
static void dim2_tasklet_fn(unsigned long data);
@@ -96,7 +85,6 @@ struct hdm_channel {
* @most_iface: most interface structure
* @capabilities: an array of channel capability data
* @io_base: I/O register base address
- * @irq_ahb0: dim2 AHB0 irq number
* @clk_speed: user selectable (through command line parameter) clock speed
* @netinfo_task: thread to deliver network status
* @netinfo_waitq: waitq for the thread to sleep
@@ -111,7 +99,6 @@ struct dim2_hdm {
struct most_interface most_iface;
char name[16 + sizeof "dim2-"];
void __iomem *io_base;
- unsigned int irq_ahb0;
int clk_speed;
struct task_struct *netinfo_task;
wait_queue_head_t netinfo_waitq;
@@ -129,10 +116,6 @@ struct dim2_hdm {
(((p)[1] == 0x18) && ((p)[2] == 0x05) && ((p)[3] == 0x0C) && \
((p)[13] == 0x3C) && ((p)[14] == 0x00) && ((p)[15] == 0x0A))
-#if defined(ENABLE_HDM_TEST)
-static struct dim2_hdm *test_dev;
-#endif
-
bool dim2_sysfs_get_state_cb(void)
{
bool state;
@@ -266,6 +249,11 @@ static int try_start_dim_transfer(struct hdm_channel *hdm_ch)
mbo = list_first_entry(head, struct mbo, list);
buf_size = mbo->buffer_length;
+ if (dim_dbr_space(&hdm_ch->ch) < buf_size) {
+ spin_unlock_irqrestore(&dim_lock, flags);
+ return -EAGAIN;
+ }
+
BUG_ON(mbo->bus_address == 0);
if (!dim_enqueue_buffer(&hdm_ch->ch, mbo->bus_address, buf_size)) {
list_del(head->next);
@@ -318,14 +306,11 @@ static int deliver_netinfo_thread(void *data)
static void retrieve_netinfo(struct dim2_hdm *dev, struct mbo *mbo)
{
u8 *data = mbo->virt_address;
- u8 *mac = dev->mac_addrs;
pr_info("Node Address: 0x%03x\n", (u16)data[16] << 8 | data[17]);
dev->link_state = data[18];
pr_info("NIState: %d\n", dev->link_state);
- memcpy(mac, data + 19, 6);
- pr_info("MAC address: %02X:%02X:%02X:%02X:%02X:%02X\n",
- mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+ memcpy(dev->mac_addrs, data + 19, 6);
dev->deliver_netinfo++;
wake_up_interruptible(&dev->netinfo_waitq);
}
@@ -423,6 +408,22 @@ static struct dim_channel **get_active_channels(struct dim2_hdm *dev,
return buffer;
}
+static irqreturn_t dim2_mlb_isr(int irq, void *_dev)
+{
+ struct dim2_hdm *dev = _dev;
+ unsigned long flags;
+
+ spin_lock_irqsave(&dim_lock, flags);
+ dim_service_mlb_int_irq();
+ spin_unlock_irqrestore(&dim_lock, flags);
+
+ if (dev->atx_idx >= 0 && dev->hch[dev->atx_idx].is_initialized)
+ while (!try_start_dim_transfer(dev->hch + dev->atx_idx))
+ continue;
+
+ return IRQ_HANDLED;
+}
+
/**
* dim2_tasklet_fn - tasklet function
* @data: private data
@@ -464,30 +465,14 @@ static irqreturn_t dim2_ahb_isr(int irq, void *_dev)
unsigned long flags;
spin_lock_irqsave(&dim_lock, flags);
- dim_service_irq(get_active_channels(dev, buffer));
+ dim_service_ahb_int_irq(get_active_channels(dev, buffer));
spin_unlock_irqrestore(&dim_lock, flags);
-#if !defined(ENABLE_HDM_TEST)
dim2_tasklet.data = (unsigned long)dev;
tasklet_schedule(&dim2_tasklet);
-#else
- dim2_tasklet_fn((unsigned long)dev);
-#endif
return IRQ_HANDLED;
}
-#if defined(ENABLE_HDM_TEST)
-
-/*
- * Utility function used by HAL-simu for calling DIM interrupt handler.
- * It is used only for TEST PURPOSE.
- */
-void raise_dim_interrupt(void)
-{
- (void)dim2_ahb_isr(0, test_dev);
-}
-#endif
-
/**
* complete_all_mbos - complete MBO's in a list
* @head: list head
@@ -557,7 +542,7 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
hdm_ch->name, buf_size, new_size);
spin_lock_irqsave(&dim_lock, flags);
hal_ret = dim_init_control(&hdm_ch->ch, is_tx, ch_addr,
- buf_size);
+ is_tx ? new_size * 2 : new_size);
break;
case MOST_CH_ASYNC:
new_size = dim_norm_ctrl_async_buffer_size(buf_size);
@@ -570,9 +555,10 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
pr_warn("%s: fixed buffer size (%d -> %d)\n",
hdm_ch->name, buf_size, new_size);
spin_lock_irqsave(&dim_lock, flags);
- hal_ret = dim_init_async(&hdm_ch->ch, is_tx, ch_addr, buf_size);
+ hal_ret = dim_init_async(&hdm_ch->ch, is_tx, ch_addr,
+ is_tx ? new_size * 2 : new_size);
break;
- case MOST_CH_ISOC_AVP:
+ case MOST_CH_ISOC:
new_size = dim_norm_isoc_buffer_size(buf_size, sub_size);
if (new_size == 0) {
pr_err("%s: invalid sub-buffer size or too small buffer size\n",
@@ -749,6 +735,7 @@ static int dim2_probe(struct platform_device *pdev)
struct resource *res;
int ret, i;
struct kobject *kobj;
+ int irq;
dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
if (!dev)
@@ -757,29 +744,37 @@ static int dim2_probe(struct platform_device *pdev)
dev->atx_idx = -1;
platform_set_drvdata(pdev, dev);
-#if defined(ENABLE_HDM_TEST)
- test_dev = dev;
-#else
res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
dev->io_base = devm_ioremap_resource(&pdev->dev, res);
if (IS_ERR(dev->io_base))
return PTR_ERR(dev->io_base);
- ret = platform_get_irq(pdev, 0);
- if (ret < 0) {
- dev_err(&pdev->dev, "failed to get irq\n");
+ irq = platform_get_irq(pdev, 0);
+ if (irq < 0) {
+ dev_err(&pdev->dev, "failed to get ahb0_int irq\n");
return -ENODEV;
}
- dev->irq_ahb0 = ret;
- ret = devm_request_irq(&pdev->dev, dev->irq_ahb0, dim2_ahb_isr, 0,
- "mlb_ahb0", dev);
+ ret = devm_request_irq(&pdev->dev, irq, dim2_ahb_isr, 0,
+ "dim2_ahb0_int", dev);
if (ret) {
- dev_err(&pdev->dev, "failed to request IRQ: %d, err: %d\n",
- dev->irq_ahb0, ret);
+ dev_err(&pdev->dev, "failed to request ahb0_int irq %d\n", irq);
return ret;
}
-#endif
+
+ irq = platform_get_irq(pdev, 1);
+ if (irq < 0) {
+ dev_err(&pdev->dev, "failed to get mlb_int irq\n");
+ return -ENODEV;
+ }
+
+ ret = devm_request_irq(&pdev->dev, irq, dim2_mlb_isr, 0,
+ "dim2_mlb_int", dev);
+ if (ret) {
+ dev_err(&pdev->dev, "failed to request mlb_int irq %d\n", irq);
+ return ret;
+ }
+
init_waitqueue_head(&dev->netinfo_waitq);
dev->deliver_netinfo = 0;
dev->netinfo_task = kthread_run(&deliver_netinfo_thread, (void *)dev,
@@ -799,7 +794,7 @@ static int dim2_probe(struct platform_device *pdev)
cap->name_suffix = hdm_ch->name;
cap->direction = MOST_CH_RX | MOST_CH_TX;
cap->data_type = MOST_CH_CONTROL | MOST_CH_ASYNC |
- MOST_CH_ISOC_AVP | MOST_CH_SYNC;
+ MOST_CH_ISOC | MOST_CH_SYNC;
cap->num_buffers_packet = MAX_BUFFERS_PACKET;
cap->buffer_size_packet = MAX_BUF_SIZE_PACKET;
cap->num_buffers_streaming = MAX_BUFFERS_STREAMING;
diff --git a/driver/hdm-dim2/dim2_reg.h b/driver/hdm-dim2/dim2_reg.h
index 3b1c200..01fe499 100644
--- a/driver/hdm-dim2/dim2_reg.h
+++ b/driver/hdm-dim2/dim2_reg.h
@@ -117,6 +117,9 @@ enum {
};
enum {
+ CDT0_RPC_SHIFT = 16 + 11,
+ CDT0_RPC_MASK = DIM2_MASK(5),
+
CDT1_BS_ISOC_SHIFT = 0,
CDT1_BS_ISOC_MASK = DIM2_MASK(9),
diff --git a/driver/hdm-dim2/dim2_sysfs.c b/driver/hdm-dim2/dim2_sysfs.c
index 2b28e4a..d8b22f9 100644
--- a/driver/hdm-dim2/dim2_sysfs.c
+++ b/driver/hdm-dim2/dim2_sysfs.c
@@ -39,7 +39,7 @@ static struct attribute *bus_default_attrs[] = {
NULL,
};
-static struct attribute_group bus_attr_group = {
+static const struct attribute_group bus_attr_group = {
.attrs = bus_default_attrs,
};