forked from Minki/linux
caif-u5500: CAIF shared memory transport protocol
Signed-off-by: Sjur Braendeland <sjur.brandeland@stericsson.com> Signed-off-by: David S. Miller <davem@davemloft.net>
This commit is contained in:
sjur.brandeland@stericsson.com
David S. Miller
parent
a10c02036f
commit
dfae55d6a5
744
drivers/net/caif/caif_shmcore.c
Normal file
744
drivers/net/caif/caif_shmcore.c
Normal file
@ -0,0 +1,744 @@
|
||||
/*
|
||||
* Copyright (C) ST-Ericsson AB 2010
|
||||
* Contact: Sjur Brendeland / sjur.brandeland@stericsson.com
|
||||
* Authors: Amarnath Revanna / amarnath.bangalore.revanna@stericsson.com,
|
||||
* Daniel Martensson / daniel.martensson@stericsson.com
|
||||
* License terms: GNU General Public License (GPL) version 2
|
||||
*/
|
||||
|
||||
#define pr_fmt(fmt) KBUILD_MODNAME ":" __func__ "():" fmt
|
||||
|
||||
#include <linux/spinlock.h>
|
||||
#include <linux/sched.h>
|
||||
#include <linux/list.h>
|
||||
#include <linux/netdevice.h>
|
||||
#include <linux/if_arp.h>
|
||||
|
||||
#include <net/caif/caif_device.h>
|
||||
#include <net/caif/caif_shm.h>
|
||||
|
||||
#define NR_TX_BUF 6
|
||||
#define NR_RX_BUF 6
|
||||
#define TX_BUF_SZ 0x2000
|
||||
#define RX_BUF_SZ 0x2000
|
||||
|
||||
#define CAIF_NEEDED_HEADROOM 32
|
||||
|
||||
#define CAIF_FLOW_ON 1
|
||||
#define CAIF_FLOW_OFF 0
|
||||
|
||||
#define LOW_WATERMARK 3
|
||||
#define HIGH_WATERMARK 4
|
||||
|
||||
/* Maximum number of CAIF buffers per shared memory buffer. */
|
||||
#define SHM_MAX_FRMS_PER_BUF 10
|
||||
|
||||
/*
|
||||
* Size in bytes of the descriptor area
|
||||
* (With end of descriptor signalling)
|
||||
*/
|
||||
#define SHM_CAIF_DESC_SIZE ((SHM_MAX_FRMS_PER_BUF + 1) * \
|
||||
sizeof(struct shm_pck_desc))
|
||||
|
||||
/*
|
||||
* Offset to the first CAIF frame within a shared memory buffer.
|
||||
* Aligned on 32 bytes.
|
||||
*/
|
||||
#define SHM_CAIF_FRM_OFS (SHM_CAIF_DESC_SIZE + (SHM_CAIF_DESC_SIZE % 32))
|
||||
|
||||
/* Number of bytes for CAIF shared memory header. */
|
||||
#define SHM_HDR_LEN 1
|
||||
|
||||
/* Number of padding bytes for the complete CAIF frame. */
|
||||
#define SHM_FRM_PAD_LEN 4
|
||||
|
||||
#define CAIF_MAX_MTU 4096
|
||||
|
||||
#define SHM_SET_FULL(x) (((x+1) & 0x0F) << 0)
|
||||
#define SHM_GET_FULL(x) (((x >> 0) & 0x0F) - 1)
|
||||
|
||||
#define SHM_SET_EMPTY(x) (((x+1) & 0x0F) << 4)
|
||||
#define SHM_GET_EMPTY(x) (((x >> 4) & 0x0F) - 1)
|
||||
|
||||
#define SHM_FULL_MASK (0x0F << 0)
|
||||
#define SHM_EMPTY_MASK (0x0F << 4)
|
||||
|
||||
struct shm_pck_desc {
|
||||
/*
|
||||
* Offset from start of shared memory area to start of
|
||||
* shared memory CAIF frame.
|
||||
*/
|
||||
u32 frm_ofs;
|
||||
u32 frm_len;
|
||||
};
|
||||
|
||||
struct buf_list {
|
||||
unsigned char *desc_vptr;
|
||||
u32 phy_addr;
|
||||
u32 index;
|
||||
u32 len;
|
||||
u32 frames;
|
||||
u32 frm_ofs;
|
||||
struct list_head list;
|
||||
};
|
||||
|
||||
struct shm_caif_frm {
|
||||
/* Number of bytes of padding before the CAIF frame. */
|
||||
u8 hdr_ofs;
|
||||
};
|
||||
|
||||
struct shmdrv_layer {
|
||||
/* caif_dev_common must always be first in the structure*/
|
||||
struct caif_dev_common cfdev;
|
||||
|
||||
u32 shm_tx_addr;
|
||||
u32 shm_rx_addr;
|
||||
u32 shm_base_addr;
|
||||
u32 tx_empty_available;
|
||||
spinlock_t lock;
|
||||
|
||||
struct list_head tx_empty_list;
|
||||
struct list_head tx_pend_list;
|
||||
struct list_head tx_full_list;
|
||||
struct list_head rx_empty_list;
|
||||
struct list_head rx_pend_list;
|
||||
struct list_head rx_full_list;
|
||||
|
||||
struct workqueue_struct *pshm_tx_workqueue;
|
||||
struct workqueue_struct *pshm_rx_workqueue;
|
||||
|
||||
struct work_struct shm_tx_work;
|
||||
struct work_struct shm_rx_work;
|
||||
|
||||
struct sk_buff_head sk_qhead;
|
||||
struct shmdev_layer *pshm_dev;
|
||||
};
|
||||
|
||||
static int shm_netdev_open(struct net_device *shm_netdev)
|
||||
{
|
||||
netif_wake_queue(shm_netdev);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int shm_netdev_close(struct net_device *shm_netdev)
|
||||
{
|
||||
netif_stop_queue(shm_netdev);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int caif_shmdrv_rx_cb(u32 mbx_msg, void *priv)
|
||||
{
|
||||
struct buf_list *pbuf;
|
||||
struct shmdrv_layer *pshm_drv;
|
||||
struct list_head *pos;
|
||||
u32 avail_emptybuff = 0;
|
||||
unsigned long flags = 0;
|
||||
|
||||
pshm_drv = (struct shmdrv_layer *)priv;
|
||||
|
||||
/* Check for received buffers. */
|
||||
if (mbx_msg & SHM_FULL_MASK) {
|
||||
int idx;
|
||||
|
||||
spin_lock_irqsave(&pshm_drv->lock, flags);
|
||||
|
||||
/* Check whether we have any outstanding buffers. */
|
||||
if (list_empty(&pshm_drv->rx_empty_list)) {
|
||||
|
||||
/* Release spin lock. */
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* We print even in IRQ context... */
|
||||
pr_warn("No empty Rx buffers to fill: "
|
||||
"mbx_msg:%x\n", mbx_msg);
|
||||
|
||||
/* Bail out. */
|
||||
goto err_sync;
|
||||
}
|
||||
|
||||
pbuf =
|
||||
list_entry(pshm_drv->rx_empty_list.next,
|
||||
struct buf_list, list);
|
||||
idx = pbuf->index;
|
||||
|
||||
/* Check buffer synchronization. */
|
||||
if (idx != SHM_GET_FULL(mbx_msg)) {
|
||||
|
||||
/* We print even in IRQ context... */
|
||||
pr_warn(
|
||||
"phyif_shm_mbx_msg_cb: RX full out of sync:"
|
||||
" idx:%d, msg:%x SHM_GET_FULL(mbx_msg):%x\n",
|
||||
idx, mbx_msg, SHM_GET_FULL(mbx_msg));
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* Bail out. */
|
||||
goto err_sync;
|
||||
}
|
||||
|
||||
list_del_init(&pbuf->list);
|
||||
list_add_tail(&pbuf->list, &pshm_drv->rx_full_list);
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* Schedule RX work queue. */
|
||||
if (!work_pending(&pshm_drv->shm_rx_work))
|
||||
queue_work(pshm_drv->pshm_rx_workqueue,
|
||||
&pshm_drv->shm_rx_work);
|
||||
}
|
||||
|
||||
/* Check for emptied buffers. */
|
||||
if (mbx_msg & SHM_EMPTY_MASK) {
|
||||
int idx;
|
||||
|
||||
spin_lock_irqsave(&pshm_drv->lock, flags);
|
||||
|
||||
/* Check whether we have any outstanding buffers. */
|
||||
if (list_empty(&pshm_drv->tx_full_list)) {
|
||||
|
||||
/* We print even in IRQ context... */
|
||||
pr_warn("No TX to empty: msg:%x\n", mbx_msg);
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* Bail out. */
|
||||
goto err_sync;
|
||||
}
|
||||
|
||||
pbuf =
|
||||
list_entry(pshm_drv->tx_full_list.next,
|
||||
struct buf_list, list);
|
||||
idx = pbuf->index;
|
||||
|
||||
/* Check buffer synchronization. */
|
||||
if (idx != SHM_GET_EMPTY(mbx_msg)) {
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* We print even in IRQ context... */
|
||||
pr_warn("TX empty "
|
||||
"out of sync:idx:%d, msg:%x\n", idx, mbx_msg);
|
||||
|
||||
/* Bail out. */
|
||||
goto err_sync;
|
||||
}
|
||||
list_del_init(&pbuf->list);
|
||||
|
||||
/* Reset buffer parameters. */
|
||||
pbuf->frames = 0;
|
||||
pbuf->frm_ofs = SHM_CAIF_FRM_OFS;
|
||||
|
||||
list_add_tail(&pbuf->list, &pshm_drv->tx_empty_list);
|
||||
|
||||
/* Check the available no. of buffers in the empty list */
|
||||
list_for_each(pos, &pshm_drv->tx_empty_list)
|
||||
avail_emptybuff++;
|
||||
|
||||
/* Check whether we have to wake up the transmitter. */
|
||||
if ((avail_emptybuff > HIGH_WATERMARK) &&
|
||||
(!pshm_drv->tx_empty_available)) {
|
||||
pshm_drv->tx_empty_available = 1;
|
||||
pshm_drv->cfdev.flowctrl
|
||||
(pshm_drv->pshm_dev->pshm_netdev,
|
||||
CAIF_FLOW_ON);
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* Schedule the work queue. if required */
|
||||
if (!work_pending(&pshm_drv->shm_tx_work))
|
||||
queue_work(pshm_drv->pshm_tx_workqueue,
|
||||
&pshm_drv->shm_tx_work);
|
||||
} else
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
err_sync:
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
static void shm_rx_work_func(struct work_struct *rx_work)
|
||||
{
|
||||
struct shmdrv_layer *pshm_drv;
|
||||
struct buf_list *pbuf;
|
||||
unsigned long flags = 0;
|
||||
struct sk_buff *skb;
|
||||
char *p;
|
||||
int ret;
|
||||
|
||||
pshm_drv = container_of(rx_work, struct shmdrv_layer, shm_rx_work);
|
||||
|
||||
while (1) {
|
||||
|
||||
struct shm_pck_desc *pck_desc;
|
||||
|
||||
spin_lock_irqsave(&pshm_drv->lock, flags);
|
||||
|
||||
/* Check for received buffers. */
|
||||
if (list_empty(&pshm_drv->rx_full_list)) {
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
break;
|
||||
}
|
||||
|
||||
pbuf =
|
||||
list_entry(pshm_drv->rx_full_list.next, struct buf_list,
|
||||
list);
|
||||
list_del_init(&pbuf->list);
|
||||
|
||||
/* Retrieve pointer to start of the packet descriptor area. */
|
||||
pck_desc = (struct shm_pck_desc *) pbuf->desc_vptr;
|
||||
|
||||
/*
|
||||
* Check whether descriptor contains a CAIF shared memory
|
||||
* frame.
|
||||
*/
|
||||
while (pck_desc->frm_ofs) {
|
||||
unsigned int frm_buf_ofs;
|
||||
unsigned int frm_pck_ofs;
|
||||
unsigned int frm_pck_len;
|
||||
/*
|
||||
* Check whether offset is within buffer limits
|
||||
* (lower).
|
||||
*/
|
||||
if (pck_desc->frm_ofs <
|
||||
(pbuf->phy_addr - pshm_drv->shm_base_addr))
|
||||
break;
|
||||
/*
|
||||
* Check whether offset is within buffer limits
|
||||
* (higher).
|
||||
*/
|
||||
if (pck_desc->frm_ofs >
|
||||
((pbuf->phy_addr - pshm_drv->shm_base_addr) +
|
||||
pbuf->len))
|
||||
break;
|
||||
|
||||
/* Calculate offset from start of buffer. */
|
||||
frm_buf_ofs =
|
||||
pck_desc->frm_ofs - (pbuf->phy_addr -
|
||||
pshm_drv->shm_base_addr);
|
||||
|
||||
/*
|
||||
* Calculate offset and length of CAIF packet while
|
||||
* taking care of the shared memory header.
|
||||
*/
|
||||
frm_pck_ofs =
|
||||
frm_buf_ofs + SHM_HDR_LEN +
|
||||
(*(pbuf->desc_vptr + frm_buf_ofs));
|
||||
frm_pck_len =
|
||||
(pck_desc->frm_len - SHM_HDR_LEN -
|
||||
(*(pbuf->desc_vptr + frm_buf_ofs)));
|
||||
|
||||
/* Check whether CAIF packet is within buffer limits */
|
||||
if ((frm_pck_ofs + pck_desc->frm_len) > pbuf->len)
|
||||
break;
|
||||
|
||||
/* Get a suitable CAIF packet and copy in data. */
|
||||
skb = netdev_alloc_skb(pshm_drv->pshm_dev->pshm_netdev,
|
||||
frm_pck_len + 1);
|
||||
BUG_ON(skb == NULL);
|
||||
|
||||
p = skb_put(skb, frm_pck_len);
|
||||
memcpy(p, pbuf->desc_vptr + frm_pck_ofs, frm_pck_len);
|
||||
|
||||
skb->protocol = htons(ETH_P_CAIF);
|
||||
skb_reset_mac_header(skb);
|
||||
skb->dev = pshm_drv->pshm_dev->pshm_netdev;
|
||||
|
||||
/* Push received packet up the stack. */
|
||||
ret = netif_rx_ni(skb);
|
||||
|
||||
if (!ret) {
|
||||
pshm_drv->pshm_dev->pshm_netdev->stats.
|
||||
rx_packets++;
|
||||
pshm_drv->pshm_dev->pshm_netdev->stats.
|
||||
rx_bytes += pck_desc->frm_len;
|
||||
} else
|
||||
++pshm_drv->pshm_dev->pshm_netdev->stats.
|
||||
rx_dropped;
|
||||
/* Move to next packet descriptor. */
|
||||
pck_desc++;
|
||||
}
|
||||
|
||||
list_add_tail(&pbuf->list, &pshm_drv->rx_pend_list);
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
}
|
||||
|
||||
/* Schedule the work queue. if required */
|
||||
if (!work_pending(&pshm_drv->shm_tx_work))
|
||||
queue_work(pshm_drv->pshm_tx_workqueue, &pshm_drv->shm_tx_work);
|
||||
|
||||
}
|
||||
|
||||
static void shm_tx_work_func(struct work_struct *tx_work)
|
||||
{
|
||||
u32 mbox_msg;
|
||||
unsigned int frmlen, avail_emptybuff, append = 0;
|
||||
unsigned long flags = 0;
|
||||
struct buf_list *pbuf = NULL;
|
||||
struct shmdrv_layer *pshm_drv;
|
||||
struct shm_caif_frm *frm;
|
||||
struct sk_buff *skb;
|
||||
struct shm_pck_desc *pck_desc;
|
||||
struct list_head *pos;
|
||||
|
||||
pshm_drv = container_of(tx_work, struct shmdrv_layer, shm_tx_work);
|
||||
|
||||
do {
|
||||
/* Initialize mailbox message. */
|
||||
mbox_msg = 0x00;
|
||||
avail_emptybuff = 0;
|
||||
|
||||
spin_lock_irqsave(&pshm_drv->lock, flags);
|
||||
|
||||
/* Check for pending receive buffers. */
|
||||
if (!list_empty(&pshm_drv->rx_pend_list)) {
|
||||
|
||||
pbuf = list_entry(pshm_drv->rx_pend_list.next,
|
||||
struct buf_list, list);
|
||||
|
||||
list_del_init(&pbuf->list);
|
||||
list_add_tail(&pbuf->list, &pshm_drv->rx_empty_list);
|
||||
/*
|
||||
* Value index is never changed,
|
||||
* so read access should be safe.
|
||||
*/
|
||||
mbox_msg |= SHM_SET_EMPTY(pbuf->index);
|
||||
}
|
||||
|
||||
skb = skb_peek(&pshm_drv->sk_qhead);
|
||||
|
||||
if (skb == NULL)
|
||||
goto send_msg;
|
||||
|
||||
/* Check the available no. of buffers in the empty list */
|
||||
list_for_each(pos, &pshm_drv->tx_empty_list)
|
||||
avail_emptybuff++;
|
||||
|
||||
if ((avail_emptybuff < LOW_WATERMARK) &&
|
||||
pshm_drv->tx_empty_available) {
|
||||
/* Update blocking condition. */
|
||||
pshm_drv->tx_empty_available = 0;
|
||||
pshm_drv->cfdev.flowctrl
|
||||
(pshm_drv->pshm_dev->pshm_netdev,
|
||||
CAIF_FLOW_OFF);
|
||||
}
|
||||
/*
|
||||
* We simply return back to the caller if we do not have space
|
||||
* either in Tx pending list or Tx empty list. In this case,
|
||||
* we hold the received skb in the skb list, waiting to
|
||||
* be transmitted once Tx buffers become available
|
||||
*/
|
||||
if (list_empty(&pshm_drv->tx_empty_list))
|
||||
goto send_msg;
|
||||
|
||||
/* Get the first free Tx buffer. */
|
||||
pbuf = list_entry(pshm_drv->tx_empty_list.next,
|
||||
struct buf_list, list);
|
||||
do {
|
||||
if (append) {
|
||||
skb = skb_peek(&pshm_drv->sk_qhead);
|
||||
if (skb == NULL)
|
||||
break;
|
||||
}
|
||||
|
||||
frm = (struct shm_caif_frm *)
|
||||
(pbuf->desc_vptr + pbuf->frm_ofs);
|
||||
|
||||
frm->hdr_ofs = 0;
|
||||
frmlen = 0;
|
||||
frmlen += SHM_HDR_LEN + frm->hdr_ofs + skb->len;
|
||||
|
||||
/* Add tail padding if needed. */
|
||||
if (frmlen % SHM_FRM_PAD_LEN)
|
||||
frmlen += SHM_FRM_PAD_LEN -
|
||||
(frmlen % SHM_FRM_PAD_LEN);
|
||||
|
||||
/*
|
||||
* Verify that packet, header and additional padding
|
||||
* can fit within the buffer frame area.
|
||||
*/
|
||||
if (frmlen >= (pbuf->len - pbuf->frm_ofs))
|
||||
break;
|
||||
|
||||
if (!append) {
|
||||
list_del_init(&pbuf->list);
|
||||
append = 1;
|
||||
}
|
||||
|
||||
skb = skb_dequeue(&pshm_drv->sk_qhead);
|
||||
/* Copy in CAIF frame. */
|
||||
skb_copy_bits(skb, 0, pbuf->desc_vptr +
|
||||
pbuf->frm_ofs + SHM_HDR_LEN +
|
||||
frm->hdr_ofs, skb->len);
|
||||
|
||||
pshm_drv->pshm_dev->pshm_netdev->stats.tx_packets++;
|
||||
pshm_drv->pshm_dev->pshm_netdev->stats.tx_bytes +=
|
||||
frmlen;
|
||||
dev_kfree_skb(skb);
|
||||
|
||||
/* Fill in the shared memory packet descriptor area. */
|
||||
pck_desc = (struct shm_pck_desc *) (pbuf->desc_vptr);
|
||||
/* Forward to current frame. */
|
||||
pck_desc += pbuf->frames;
|
||||
pck_desc->frm_ofs = (pbuf->phy_addr -
|
||||
pshm_drv->shm_base_addr) +
|
||||
pbuf->frm_ofs;
|
||||
pck_desc->frm_len = frmlen;
|
||||
/* Terminate packet descriptor area. */
|
||||
pck_desc++;
|
||||
pck_desc->frm_ofs = 0;
|
||||
/* Update buffer parameters. */
|
||||
pbuf->frames++;
|
||||
pbuf->frm_ofs += frmlen + (frmlen % 32);
|
||||
|
||||
} while (pbuf->frames < SHM_MAX_FRMS_PER_BUF);
|
||||
|
||||
/* Assign buffer as full. */
|
||||
list_add_tail(&pbuf->list, &pshm_drv->tx_full_list);
|
||||
append = 0;
|
||||
mbox_msg |= SHM_SET_FULL(pbuf->index);
|
||||
send_msg:
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
if (mbox_msg)
|
||||
pshm_drv->pshm_dev->pshmdev_mbxsend
|
||||
(pshm_drv->pshm_dev->shm_id, mbox_msg);
|
||||
} while (mbox_msg);
|
||||
}
|
||||
|
||||
static int shm_netdev_tx(struct sk_buff *skb, struct net_device *shm_netdev)
|
||||
{
|
||||
struct shmdrv_layer *pshm_drv;
|
||||
unsigned long flags = 0;
|
||||
|
||||
pshm_drv = netdev_priv(shm_netdev);
|
||||
|
||||
spin_lock_irqsave(&pshm_drv->lock, flags);
|
||||
|
||||
skb_queue_tail(&pshm_drv->sk_qhead, skb);
|
||||
|
||||
spin_unlock_irqrestore(&pshm_drv->lock, flags);
|
||||
|
||||
/* Schedule Tx work queue. for deferred processing of skbs*/
|
||||
if (!work_pending(&pshm_drv->shm_tx_work))
|
||||
queue_work(pshm_drv->pshm_tx_workqueue, &pshm_drv->shm_tx_work);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct net_device_ops netdev_ops = {
|
||||
.ndo_open = shm_netdev_open,
|
||||
.ndo_stop = shm_netdev_close,
|
||||
.ndo_start_xmit = shm_netdev_tx,
|
||||
};
|
||||
|
||||
static void shm_netdev_setup(struct net_device *pshm_netdev)
|
||||
{
|
||||
struct shmdrv_layer *pshm_drv;
|
||||
pshm_netdev->netdev_ops = &netdev_ops;
|
||||
|
||||
pshm_netdev->mtu = CAIF_MAX_MTU;
|
||||
pshm_netdev->type = ARPHRD_CAIF;
|
||||
pshm_netdev->hard_header_len = CAIF_NEEDED_HEADROOM;
|
||||
pshm_netdev->tx_queue_len = 0;
|
||||
pshm_netdev->destructor = free_netdev;
|
||||
|
||||
pshm_drv = netdev_priv(pshm_netdev);
|
||||
|
||||
/* Initialize structures in a clean state. */
|
||||
memset(pshm_drv, 0, sizeof(struct shmdrv_layer));
|
||||
|
||||
pshm_drv->cfdev.link_select = CAIF_LINK_LOW_LATENCY;
|
||||
}
|
||||
|
||||
int caif_shmcore_probe(struct shmdev_layer *pshm_dev)
|
||||
{
|
||||
int result, j;
|
||||
struct shmdrv_layer *pshm_drv = NULL;
|
||||
|
||||
pshm_dev->pshm_netdev = alloc_netdev(sizeof(struct shmdrv_layer),
|
||||
"cfshm%d", shm_netdev_setup);
|
||||
if (!pshm_dev->pshm_netdev)
|
||||
return -ENOMEM;
|
||||
|
||||
pshm_drv = netdev_priv(pshm_dev->pshm_netdev);
|
||||
pshm_drv->pshm_dev = pshm_dev;
|
||||
|
||||
/*
|
||||
* Initialization starts with the verification of the
|
||||
* availability of MBX driver by calling its setup function.
|
||||
* MBX driver must be available by this time for proper
|
||||
* functioning of SHM driver.
|
||||
*/
|
||||
if ((pshm_dev->pshmdev_mbxsetup
|
||||
(caif_shmdrv_rx_cb, pshm_dev, pshm_drv)) != 0) {
|
||||
pr_warn("Could not config. SHM Mailbox,"
|
||||
" Bailing out.....\n");
|
||||
free_netdev(pshm_dev->pshm_netdev);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
skb_queue_head_init(&pshm_drv->sk_qhead);
|
||||
|
||||
pr_info("SHM DEVICE[%d] PROBED BY DRIVER, NEW SHM DRIVER"
|
||||
" INSTANCE AT pshm_drv =0x%p\n",
|
||||
pshm_drv->pshm_dev->shm_id, pshm_drv);
|
||||
|
||||
if (pshm_dev->shm_total_sz <
|
||||
(NR_TX_BUF * TX_BUF_SZ + NR_RX_BUF * RX_BUF_SZ)) {
|
||||
|
||||
pr_warn("ERROR, Amount of available"
|
||||
" Phys. SHM cannot accomodate current SHM "
|
||||
"driver configuration, Bailing out ...\n");
|
||||
free_netdev(pshm_dev->pshm_netdev);
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
pshm_drv->shm_base_addr = pshm_dev->shm_base_addr;
|
||||
pshm_drv->shm_tx_addr = pshm_drv->shm_base_addr;
|
||||
|
||||
if (pshm_dev->shm_loopback)
|
||||
pshm_drv->shm_rx_addr = pshm_drv->shm_tx_addr;
|
||||
else
|
||||
pshm_drv->shm_rx_addr = pshm_dev->shm_base_addr +
|
||||
(NR_TX_BUF * TX_BUF_SZ);
|
||||
|
||||
INIT_LIST_HEAD(&pshm_drv->tx_empty_list);
|
||||
INIT_LIST_HEAD(&pshm_drv->tx_pend_list);
|
||||
INIT_LIST_HEAD(&pshm_drv->tx_full_list);
|
||||
|
||||
INIT_LIST_HEAD(&pshm_drv->rx_empty_list);
|
||||
INIT_LIST_HEAD(&pshm_drv->rx_pend_list);
|
||||
INIT_LIST_HEAD(&pshm_drv->rx_full_list);
|
||||
|
||||
INIT_WORK(&pshm_drv->shm_tx_work, shm_tx_work_func);
|
||||
INIT_WORK(&pshm_drv->shm_rx_work, shm_rx_work_func);
|
||||
|
||||
pshm_drv->pshm_tx_workqueue =
|
||||
create_singlethread_workqueue("shm_tx_work");
|
||||
pshm_drv->pshm_rx_workqueue =
|
||||
create_singlethread_workqueue("shm_rx_work");
|
||||
|
||||
for (j = 0; j < NR_TX_BUF; j++) {
|
||||
struct buf_list *tx_buf =
|
||||
kmalloc(sizeof(struct buf_list), GFP_KERNEL);
|
||||
|
||||
if (tx_buf == NULL) {
|
||||
pr_warn("ERROR, Could not"
|
||||
" allocate dynamic mem. for tx_buf,"
|
||||
" Bailing out ...\n");
|
||||
free_netdev(pshm_dev->pshm_netdev);
|
||||
return -ENOMEM;
|
||||
}
|
||||
tx_buf->index = j;
|
||||
tx_buf->phy_addr = pshm_drv->shm_tx_addr + (TX_BUF_SZ * j);
|
||||
tx_buf->len = TX_BUF_SZ;
|
||||
tx_buf->frames = 0;
|
||||
tx_buf->frm_ofs = SHM_CAIF_FRM_OFS;
|
||||
|
||||
if (pshm_dev->shm_loopback)
|
||||
tx_buf->desc_vptr = (char *)tx_buf->phy_addr;
|
||||
else
|
||||
tx_buf->desc_vptr =
|
||||
ioremap(tx_buf->phy_addr, TX_BUF_SZ);
|
||||
|
||||
list_add_tail(&tx_buf->list, &pshm_drv->tx_empty_list);
|
||||
}
|
||||
|
||||
for (j = 0; j < NR_RX_BUF; j++) {
|
||||
struct buf_list *rx_buf =
|
||||
kmalloc(sizeof(struct buf_list), GFP_KERNEL);
|
||||
|
||||
if (rx_buf == NULL) {
|
||||
pr_warn("ERROR, Could not"
|
||||
" allocate dynamic mem.for rx_buf,"
|
||||
" Bailing out ...\n");
|
||||
free_netdev(pshm_dev->pshm_netdev);
|
||||
return -ENOMEM;
|
||||
}
|
||||
rx_buf->index = j;
|
||||
rx_buf->phy_addr = pshm_drv->shm_rx_addr + (RX_BUF_SZ * j);
|
||||
rx_buf->len = RX_BUF_SZ;
|
||||
|
||||
if (pshm_dev->shm_loopback)
|
||||
rx_buf->desc_vptr = (char *)rx_buf->phy_addr;
|
||||
else
|
||||
rx_buf->desc_vptr =
|
||||
ioremap(rx_buf->phy_addr, RX_BUF_SZ);
|
||||
list_add_tail(&rx_buf->list, &pshm_drv->rx_empty_list);
|
||||
}
|
||||
|
||||
pshm_drv->tx_empty_available = 1;
|
||||
result = register_netdev(pshm_dev->pshm_netdev);
|
||||
if (result)
|
||||
pr_warn("ERROR[%d], SHM could not, "
|
||||
"register with NW FRMWK Bailing out ...\n", result);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void caif_shmcore_remove(struct net_device *pshm_netdev)
|
||||
{
|
||||
struct buf_list *pbuf;
|
||||
struct shmdrv_layer *pshm_drv = NULL;
|
||||
|
||||
pshm_drv = netdev_priv(pshm_netdev);
|
||||
|
||||
while (!(list_empty(&pshm_drv->tx_pend_list))) {
|
||||
pbuf =
|
||||
list_entry(pshm_drv->tx_pend_list.next,
|
||||
struct buf_list, list);
|
||||
|
||||
list_del(&pbuf->list);
|
||||
kfree(pbuf);
|
||||
}
|
||||
|
||||
while (!(list_empty(&pshm_drv->tx_full_list))) {
|
||||
pbuf =
|
||||
list_entry(pshm_drv->tx_full_list.next,
|
||||
struct buf_list, list);
|
||||
list_del(&pbuf->list);
|
||||
kfree(pbuf);
|
||||
}
|
||||
|
||||
while (!(list_empty(&pshm_drv->tx_empty_list))) {
|
||||
pbuf =
|
||||
list_entry(pshm_drv->tx_empty_list.next,
|
||||
struct buf_list, list);
|
||||
list_del(&pbuf->list);
|
||||
kfree(pbuf);
|
||||
}
|
||||
|
||||
while (!(list_empty(&pshm_drv->rx_full_list))) {
|
||||
pbuf =
|
||||
list_entry(pshm_drv->tx_full_list.next,
|
||||
struct buf_list, list);
|
||||
list_del(&pbuf->list);
|
||||
kfree(pbuf);
|
||||
}
|
||||
|
||||
while (!(list_empty(&pshm_drv->rx_pend_list))) {
|
||||
pbuf =
|
||||
list_entry(pshm_drv->tx_pend_list.next,
|
||||
struct buf_list, list);
|
||||
list_del(&pbuf->list);
|
||||
kfree(pbuf);
|
||||
}
|
||||
|
||||
while (!(list_empty(&pshm_drv->rx_empty_list))) {
|
||||
pbuf =
|
||||
list_entry(pshm_drv->rx_empty_list.next,
|
||||
struct buf_list, list);
|
||||
list_del(&pbuf->list);
|
||||
kfree(pbuf);
|
||||
}
|
||||
|
||||
/* Destroy work queues. */
|
||||
destroy_workqueue(pshm_drv->pshm_tx_workqueue);
|
||||
destroy_workqueue(pshm_drv->pshm_rx_workqueue);
|
||||
|
||||
unregister_netdev(pshm_netdev);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user