Orange Pi5 kernel

// SPDX-License-Identifier: GPL-2.0-only
/*
 *
 * Author	Karsten Keil <kkeil@novell.com>
 *
 * Copyright 2008  by Karsten Keil <kkeil@novell.com>
 */
 
#include <linux/gfp.h>
#include <linux/module.h>
#include <linux/mISDNhw.h>
 
static void
dchannel_bh(struct work_struct *ws)
{
<------>struct dchannel	*dch  = container_of(ws, struct dchannel, workq);
<------>struct sk_buff	*skb;
<------>int		err;
 
<------>if (test_and_clear_bit(FLG_RECVQUEUE, &dch->Flags)) {
<------><------>while ((skb = skb_dequeue(&dch->rqueue))) {
<------><------><------>if (likely(dch->dev.D.peer)) {
<------><------><------><------>err = dch->dev.D.recv(dch->dev.D.peer, skb);
<------><------><------><------>if (err)
<------><------><------><------><------>dev_kfree_skb(skb);
<------><------><------>} else
<------><------><------><------>dev_kfree_skb(skb);
<------><------>}
<------>}
<------>if (test_and_clear_bit(FLG_PHCHANGE, &dch->Flags)) {
<------><------>if (dch->phfunc)
<------><------><------>dch->phfunc(dch);
<------>}
}
 
static void
bchannel_bh(struct work_struct *ws)
{
<------>struct bchannel	*bch  = container_of(ws, struct bchannel, workq);
<------>struct sk_buff	*skb;
<------>int		err;
 
<------>if (test_and_clear_bit(FLG_RECVQUEUE, &bch->Flags)) {
<------><------>while ((skb = skb_dequeue(&bch->rqueue))) {
<------><------><------>bch->rcount--;
<------><------><------>if (likely(bch->ch.peer)) {
<------><------><------><------>err = bch->ch.recv(bch->ch.peer, skb);
<------><------><------><------>if (err)
<------><------><------><------><------>dev_kfree_skb(skb);
<------><------><------>} else
<------><------><------><------>dev_kfree_skb(skb);
<------><------>}
<------>}
}
 
int
mISDN_initdchannel(struct dchannel *ch, int maxlen, void *phf)
{
<------>test_and_set_bit(FLG_HDLC, &ch->Flags);
<------>ch->maxlen = maxlen;
<------>ch->hw = NULL;
<------>ch->rx_skb = NULL;
<------>ch->tx_skb = NULL;
<------>ch->tx_idx = 0;
<------>ch->phfunc = phf;
<------>skb_queue_head_init(&ch->squeue);
<------>skb_queue_head_init(&ch->rqueue);
<------>INIT_LIST_HEAD(&ch->dev.bchannels);
<------>INIT_WORK(&ch->workq, dchannel_bh);
<------>return 0;
}
EXPORT_SYMBOL(mISDN_initdchannel);
 
int
mISDN_initbchannel(struct bchannel *ch, unsigned short maxlen,
<------><------>   unsigned short minlen)
{
<------>ch->Flags = 0;
<------>ch->minlen = minlen;
<------>ch->next_minlen = minlen;
<------>ch->init_minlen = minlen;
<------>ch->maxlen = maxlen;
<------>ch->next_maxlen = maxlen;
<------>ch->init_maxlen = maxlen;
<------>ch->hw = NULL;
<------>ch->rx_skb = NULL;
<------>ch->tx_skb = NULL;
<------>ch->tx_idx = 0;
<------>skb_queue_head_init(&ch->rqueue);
<------>ch->rcount = 0;
<------>ch->next_skb = NULL;
<------>INIT_WORK(&ch->workq, bchannel_bh);
<------>return 0;
}
EXPORT_SYMBOL(mISDN_initbchannel);
 
int
mISDN_freedchannel(struct dchannel *ch)
{
<------>if (ch->tx_skb) {
<------><------>dev_kfree_skb(ch->tx_skb);
<------><------>ch->tx_skb = NULL;
<------>}
<------>if (ch->rx_skb) {
<------><------>dev_kfree_skb(ch->rx_skb);
<------><------>ch->rx_skb = NULL;
<------>}
<------>skb_queue_purge(&ch->squeue);
<------>skb_queue_purge(&ch->rqueue);
<------>flush_work(&ch->workq);
<------>return 0;
}
EXPORT_SYMBOL(mISDN_freedchannel);
 
void
mISDN_clear_bchannel(struct bchannel *ch)
{
<------>if (ch->tx_skb) {
<------><------>dev_kfree_skb(ch->tx_skb);
<------><------>ch->tx_skb = NULL;
<------>}
<------>ch->tx_idx = 0;
<------>if (ch->rx_skb) {
<------><------>dev_kfree_skb(ch->rx_skb);
<------><------>ch->rx_skb = NULL;
<------>}
<------>if (ch->next_skb) {
<------><------>dev_kfree_skb(ch->next_skb);
<------><------>ch->next_skb = NULL;
<------>}
<------>test_and_clear_bit(FLG_TX_BUSY, &ch->Flags);
<------>test_and_clear_bit(FLG_TX_NEXT, &ch->Flags);
<------>test_and_clear_bit(FLG_ACTIVE, &ch->Flags);
<------>test_and_clear_bit(FLG_FILLEMPTY, &ch->Flags);
<------>test_and_clear_bit(FLG_TX_EMPTY, &ch->Flags);
<------>test_and_clear_bit(FLG_RX_OFF, &ch->Flags);
<------>ch->dropcnt = 0;
<------>ch->minlen = ch->init_minlen;
<------>ch->next_minlen = ch->init_minlen;
<------>ch->maxlen = ch->init_maxlen;
<------>ch->next_maxlen = ch->init_maxlen;
<------>skb_queue_purge(&ch->rqueue);
<------>ch->rcount = 0;
}
EXPORT_SYMBOL(mISDN_clear_bchannel);
 
void
mISDN_freebchannel(struct bchannel *ch)
{
<------>cancel_work_sync(&ch->workq);
<------>mISDN_clear_bchannel(ch);
}
EXPORT_SYMBOL(mISDN_freebchannel);
 
int
mISDN_ctrl_bchannel(struct bchannel *bch, struct mISDN_ctrl_req *cq)
{
<------>int ret = 0;
 
<------>switch (cq->op) {
<------>case MISDN_CTRL_GETOP:
<------><------>cq->op = MISDN_CTRL_RX_BUFFER | MISDN_CTRL_FILL_EMPTY |
<------><------><------> MISDN_CTRL_RX_OFF;
<------><------>break;
<------>case MISDN_CTRL_FILL_EMPTY:
<------><------>if (cq->p1) {
<------><------><------>memset(bch->fill, cq->p2 & 0xff, MISDN_BCH_FILL_SIZE);
<------><------><------>test_and_set_bit(FLG_FILLEMPTY, &bch->Flags);
<------><------>} else {
<------><------><------>test_and_clear_bit(FLG_FILLEMPTY, &bch->Flags);
<------><------>}
<------><------>break;
<------>case MISDN_CTRL_RX_OFF:
<------><------>/* read back dropped byte count */
<------><------>cq->p2 = bch->dropcnt;
<------><------>if (cq->p1)
<------><------><------>test_and_set_bit(FLG_RX_OFF, &bch->Flags);
<------><------>else
<------><------><------>test_and_clear_bit(FLG_RX_OFF, &bch->Flags);
<------><------>bch->dropcnt = 0;
<------><------>break;
<------>case MISDN_CTRL_RX_BUFFER:
<------><------>if (cq->p2 > MISDN_CTRL_RX_SIZE_IGNORE)
<------><------><------>bch->next_maxlen = cq->p2;
<------><------>if (cq->p1 > MISDN_CTRL_RX_SIZE_IGNORE)
<------><------><------>bch->next_minlen = cq->p1;
<------><------>/* we return the old values */
<------><------>cq->p1 = bch->minlen;
<------><------>cq->p2 = bch->maxlen;
<------><------>break;
<------>default:
<------><------>pr_info("mISDN unhandled control %x operation\n", cq->op);
<------><------>ret = -EINVAL;
<------><------>break;
<------>}
<------>return ret;
}
EXPORT_SYMBOL(mISDN_ctrl_bchannel);
 
static inline u_int
get_sapi_tei(u_char *p)
{
<------>u_int	sapi, tei;
 
<------>sapi = *p >> 2;
<------>tei = p[1] >> 1;
<------>return sapi | (tei << 8);
}
 
void
recv_Dchannel(struct dchannel *dch)
{
<------>struct mISDNhead *hh;
 
<------>if (dch->rx_skb->len < 2) { /* at least 2 for sapi / tei */
<------><------>dev_kfree_skb(dch->rx_skb);
<------><------>dch->rx_skb = NULL;
<------><------>return;
<------>}
<------>hh = mISDN_HEAD_P(dch->rx_skb);
<------>hh->prim = PH_DATA_IND;
<------>hh->id = get_sapi_tei(dch->rx_skb->data);
<------>skb_queue_tail(&dch->rqueue, dch->rx_skb);
<------>dch->rx_skb = NULL;
<------>schedule_event(dch, FLG_RECVQUEUE);
}
EXPORT_SYMBOL(recv_Dchannel);
 
void
recv_Echannel(struct dchannel *ech, struct dchannel *dch)
{
<------>struct mISDNhead *hh;
 
<------>if (ech->rx_skb->len < 2) { /* at least 2 for sapi / tei */
<------><------>dev_kfree_skb(ech->rx_skb);
<------><------>ech->rx_skb = NULL;
<------><------>return;
<------>}
<------>hh = mISDN_HEAD_P(ech->rx_skb);
<------>hh->prim = PH_DATA_E_IND;
<------>hh->id = get_sapi_tei(ech->rx_skb->data);
<------>skb_queue_tail(&dch->rqueue, ech->rx_skb);
<------>ech->rx_skb = NULL;
<------>schedule_event(dch, FLG_RECVQUEUE);
}
EXPORT_SYMBOL(recv_Echannel);
 
void
recv_Bchannel(struct bchannel *bch, unsigned int id, bool force)
{
<------>struct mISDNhead *hh;
 
<------>/* if allocation did fail upper functions still may call us */
<------>if (unlikely(!bch->rx_skb))
<------><------>return;
<------>if (unlikely(!bch->rx_skb->len)) {
<------><------>/* we have no data to send - this may happen after recovery
<------><------> * from overflow or too small allocation.
<------><------> * We need to free the buffer here */
<------><------>dev_kfree_skb(bch->rx_skb);
<------><------>bch->rx_skb = NULL;
<------>} else {
<------><------>if (test_bit(FLG_TRANSPARENT, &bch->Flags) &&
<------><------>    (bch->rx_skb->len < bch->minlen) && !force)
<------><------><------><------>return;
<------><------>hh = mISDN_HEAD_P(bch->rx_skb);
<------><------>hh->prim = PH_DATA_IND;
<------><------>hh->id = id;
<------><------>if (bch->rcount >= 64) {
<------><------><------>printk(KERN_WARNING
<------><------><------>       "B%d receive queue overflow - flushing!\n",
<------><------><------>       bch->nr);
<------><------><------>skb_queue_purge(&bch->rqueue);
<------><------>}
<------><------>bch->rcount++;
<------><------>skb_queue_tail(&bch->rqueue, bch->rx_skb);
<------><------>bch->rx_skb = NULL;
<------><------>schedule_event(bch, FLG_RECVQUEUE);
<------>}
}
EXPORT_SYMBOL(recv_Bchannel);
 
void
recv_Dchannel_skb(struct dchannel *dch, struct sk_buff *skb)
{
<------>skb_queue_tail(&dch->rqueue, skb);
<------>schedule_event(dch, FLG_RECVQUEUE);
}
EXPORT_SYMBOL(recv_Dchannel_skb);
 
void
recv_Bchannel_skb(struct bchannel *bch, struct sk_buff *skb)
{
<------>if (bch->rcount >= 64) {
<------><------>printk(KERN_WARNING "B-channel %p receive queue overflow, "
<------><------>       "flushing!\n", bch);
<------><------>skb_queue_purge(&bch->rqueue);
<------><------>bch->rcount = 0;
<------>}
<------>bch->rcount++;
<------>skb_queue_tail(&bch->rqueue, skb);
<------>schedule_event(bch, FLG_RECVQUEUE);
}
EXPORT_SYMBOL(recv_Bchannel_skb);
 
static void
confirm_Dsend(struct dchannel *dch)
{
<------>struct sk_buff	*skb;
 
<------>skb = _alloc_mISDN_skb(PH_DATA_CNF, mISDN_HEAD_ID(dch->tx_skb),
<------><------><------>       0, NULL, GFP_ATOMIC);
<------>if (!skb) {
<------><------>printk(KERN_ERR "%s: no skb id %x\n", __func__,
<------><------>       mISDN_HEAD_ID(dch->tx_skb));
<------><------>return;
<------>}
<------>skb_queue_tail(&dch->rqueue, skb);
<------>schedule_event(dch, FLG_RECVQUEUE);
}
 
int
get_next_dframe(struct dchannel *dch)
{
<------>dch->tx_idx = 0;
<------>dch->tx_skb = skb_dequeue(&dch->squeue);
<------>if (dch->tx_skb) {
<------><------>confirm_Dsend(dch);
<------><------>return 1;
<------>}
<------>dch->tx_skb = NULL;
<------>test_and_clear_bit(FLG_TX_BUSY, &dch->Flags);
<------>return 0;
}
EXPORT_SYMBOL(get_next_dframe);
 
static void
confirm_Bsend(struct bchannel *bch)
{
<------>struct sk_buff	*skb;
 
<------>if (bch->rcount >= 64) {
<------><------>printk(KERN_WARNING "B-channel %p receive queue overflow, "
<------><------>       "flushing!\n", bch);
<------><------>skb_queue_purge(&bch->rqueue);
<------><------>bch->rcount = 0;
<------>}
<------>skb = _alloc_mISDN_skb(PH_DATA_CNF, mISDN_HEAD_ID(bch->tx_skb),
<------><------><------>       0, NULL, GFP_ATOMIC);
<------>if (!skb) {
<------><------>printk(KERN_ERR "%s: no skb id %x\n", __func__,
<------><------>       mISDN_HEAD_ID(bch->tx_skb));
<------><------>return;
<------>}
<------>bch->rcount++;
<------>skb_queue_tail(&bch->rqueue, skb);
<------>schedule_event(bch, FLG_RECVQUEUE);
}
 
int
get_next_bframe(struct bchannel *bch)
{
<------>bch->tx_idx = 0;
<------>if (test_bit(FLG_TX_NEXT, &bch->Flags)) {
<------><------>bch->tx_skb = bch->next_skb;
<------><------>if (bch->tx_skb) {
<------><------><------>bch->next_skb = NULL;
<------><------><------>test_and_clear_bit(FLG_TX_NEXT, &bch->Flags);
<------><------><------>/* confirm imediately to allow next data */
<------><------><------>confirm_Bsend(bch);
<------><------><------>return 1;
<------><------>} else {
<------><------><------>test_and_clear_bit(FLG_TX_NEXT, &bch->Flags);
<------><------><------>printk(KERN_WARNING "B TX_NEXT without skb\n");
<------><------>}
<------>}
<------>bch->tx_skb = NULL;
<------>test_and_clear_bit(FLG_TX_BUSY, &bch->Flags);
<------>return 0;
}
EXPORT_SYMBOL(get_next_bframe);
 
void
queue_ch_frame(struct mISDNchannel *ch, u_int pr, int id, struct sk_buff *skb)
{
<------>struct mISDNhead *hh;
 
<------>if (!skb) {
<------><------>_queue_data(ch, pr, id, 0, NULL, GFP_ATOMIC);
<------>} else {
<------><------>if (ch->peer) {
<------><------><------>hh = mISDN_HEAD_P(skb);
<------><------><------>hh->prim = pr;
<------><------><------>hh->id = id;
<------><------><------>if (!ch->recv(ch->peer, skb))
<------><------><------><------>return;
<------><------>}
<------><------>dev_kfree_skb(skb);
<------>}
}
EXPORT_SYMBOL(queue_ch_frame);
 
int
dchannel_senddata(struct dchannel *ch, struct sk_buff *skb)
{
<------>/* check oversize */
<------>if (skb->len <= 0) {
<------><------>printk(KERN_WARNING "%s: skb too small\n", __func__);
<------><------>return -EINVAL;
<------>}
<------>if (skb->len > ch->maxlen) {
<------><------>printk(KERN_WARNING "%s: skb too large(%d/%d)\n",
<------><------>       __func__, skb->len, ch->maxlen);
<------><------>return -EINVAL;
<------>}
<------>/* HW lock must be obtained */
<------>if (test_and_set_bit(FLG_TX_BUSY, &ch->Flags)) {
<------><------>skb_queue_tail(&ch->squeue, skb);
<------><------>return 0;
<------>} else {
<------><------>/* write to fifo */
<------><------>ch->tx_skb = skb;
<------><------>ch->tx_idx = 0;
<------><------>return 1;
<------>}
}
EXPORT_SYMBOL(dchannel_senddata);
 
int
bchannel_senddata(struct bchannel *ch, struct sk_buff *skb)
{
 
<------>/* check oversize */
<------>if (skb->len <= 0) {
<------><------>printk(KERN_WARNING "%s: skb too small\n", __func__);
<------><------>return -EINVAL;
<------>}
<------>if (skb->len > ch->maxlen) {
<------><------>printk(KERN_WARNING "%s: skb too large(%d/%d)\n",
<------><------>       __func__, skb->len, ch->maxlen);
<------><------>return -EINVAL;
<------>}
<------>/* HW lock must be obtained */
<------>/* check for pending next_skb */
<------>if (ch->next_skb) {
<------><------>printk(KERN_WARNING
<------><------>       "%s: next_skb exist ERROR (skb->len=%d next_skb->len=%d)\n",
<------><------>       __func__, skb->len, ch->next_skb->len);
<------><------>return -EBUSY;
<------>}
<------>if (test_and_set_bit(FLG_TX_BUSY, &ch->Flags)) {
<------><------>test_and_set_bit(FLG_TX_NEXT, &ch->Flags);
<------><------>ch->next_skb = skb;
<------><------>return 0;
<------>} else {
<------><------>/* write to fifo */
<------><------>ch->tx_skb = skb;
<------><------>ch->tx_idx = 0;
<------><------>confirm_Bsend(ch);
<------><------>return 1;
<------>}
}
EXPORT_SYMBOL(bchannel_senddata);
 
/* The function allocates a new receive skb on demand with a size for the
 * requirements of the current protocol. It returns the tailroom of the
 * receive skb or an error.
 */
int
bchannel_get_rxbuf(struct bchannel *bch, int reqlen)
{
<------>int len;
 
<------>if (bch->rx_skb) {
<------><------>len = skb_tailroom(bch->rx_skb);
<------><------>if (len < reqlen) {
<------><------><------>pr_warn("B%d no space for %d (only %d) bytes\n",
<------><------><------><------>bch->nr, reqlen, len);
<------><------><------>if (test_bit(FLG_TRANSPARENT, &bch->Flags)) {
<------><------><------><------>/* send what we have now and try a new buffer */
<------><------><------><------>recv_Bchannel(bch, 0, true);
<------><------><------>} else {
<------><------><------><------>/* on HDLC we have to drop too big frames */
<------><------><------><------>return -EMSGSIZE;
<------><------><------>}
<------><------>} else {
<------><------><------>return len;
<------><------>}
<------>}
<------>/* update current min/max length first */
<------>if (unlikely(bch->maxlen != bch->next_maxlen))
<------><------>bch->maxlen = bch->next_maxlen;
<------>if (unlikely(bch->minlen != bch->next_minlen))
<------><------>bch->minlen = bch->next_minlen;
<------>if (unlikely(reqlen > bch->maxlen))
<------><------>return -EMSGSIZE;
<------>if (test_bit(FLG_TRANSPARENT, &bch->Flags)) {
<------><------>if (reqlen >= bch->minlen) {
<------><------><------>len = reqlen;
<------><------>} else {
<------><------><------>len = 2 * bch->minlen;
<------><------><------>if (len > bch->maxlen)
<------><------><------><------>len = bch->maxlen;
<------><------>}
<------>} else {
<------><------>/* with HDLC we do not know the length yet */
<------><------>len = bch->maxlen;
<------>}
<------>bch->rx_skb = mI_alloc_skb(len, GFP_ATOMIC);
<------>if (!bch->rx_skb) {
<------><------>pr_warn("B%d receive no memory for %d bytes\n", bch->nr, len);
<------><------>len = -ENOMEM;
<------>}
<------>return len;
}
EXPORT_SYMBOL(bchannel_get_rxbuf);