static void raw_rcv(struct sk_buff *skb, void *data)
{
struct sock *sk = (struct sock*)data;
struct raw_opt *ro = raw_sk(sk);
struct sockaddr_can *addr;
int error;
DBG("received skbuff %p, sk %p\n", skb, sk);
DBG_SKB(skb);
if (!ro->recv_own_msgs) {
/* check the received tx sock reference */
if (*(struct sock **)skb->cb == sk) {
DBG("trashed own tx msg\n");
kfree_skb(skb);
return;
}
}
addr = (struct sockaddr_can *)skb->cb;
memset(addr, 0, sizeof(*addr));
addr->can_family = AF_CAN;
addr->can_ifindex = skb->dev->ifindex;
error = sock_queue_rcv_skb(sk, skb);
if (error < 0) {
DBG("sock_queue_rcv_skb failed: %d\n", error);
DBG("freeing skbuff %p\n", skb);
kfree_skb(skb);
}
}
static void fritz_b_l2l1(struct hisax_if *ifc, int pr, void *arg)
{
struct fritz_bcs *bcs = ifc->priv;
struct sk_buff *skb = arg;
int mode;
DBG(0x10, "pr %#x", pr);
switch (pr) {
case PH_DATA | REQUEST:
if (bcs->tx_skb)
BUG();
bcs->tx_skb = skb;
DBG_SKB(1, skb);
hdlc_fill_fifo(bcs);
break;
case PH_ACTIVATE | REQUEST:
mode = (int) arg;
DBG(4,"B%d,PH_ACTIVATE_REQUEST %d", bcs->channel + 1, mode);
modehdlc(bcs, mode);
B_L1L2(bcs, PH_ACTIVATE | INDICATION, NULL);
break;
case PH_DEACTIVATE | REQUEST:
DBG(4,"B%d,PH_DEACTIVATE_REQUEST", bcs->channel + 1);
modehdlc(bcs, L1_MODE_NULL);
B_L1L2(bcs, PH_DEACTIVATE | INDICATION, NULL);
break;
}
}
static int raw_sendmsg(struct kiocb *iocb, struct socket *sock,
struct msghdr *msg, size_t size)
{
struct sock *sk = sock->sk;
struct raw_opt *ro = raw_sk(sk);
struct sk_buff *skb;
struct net_device *dev;
int ifindex;
int err;
DBG("socket %p, sk %p\n", sock, sk);
if (msg->msg_name) {
struct sockaddr_can *addr =
(struct sockaddr_can *)msg->msg_name;
if (addr->can_family != AF_CAN)
return -EINVAL;
ifindex = addr->can_ifindex;
} else
ifindex = ro->ifindex;
dev = dev_get_by_index(ifindex);
if (!dev) {
DBG("device %d not found\n", ifindex);
return -ENXIO;
}
skb = alloc_skb(size, GFP_KERNEL);
if (!skb) {
dev_put(dev);
return -ENOMEM;
}
err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
if (err < 0) {
kfree_skb(skb);
dev_put(dev);
return err;
}
skb->dev = dev;
skb->sk = sk;
DBG("sending skbuff to interface %d\n", ifindex);
DBG_SKB(skb);
err = can_send(skb, ro->loopback);
dev_put(dev);
if (err)
return err;
return size;
}
static void dout_start_xmit(struct FsmInst *fsm, int event, void *arg)
{
// FIXME unify?
struct st5481_adapter *adapter = fsm->userdata;
struct st5481_d_out *d_out = &adapter->d_out;
struct urb *urb;
int len, bytes_sent;
struct sk_buff *skb;
int buf_nr = 0;
skb = d_out->tx_skb;
DBG(2,"len=%d",skb->len);
isdnhdlc_out_init(&d_out->hdlc_state, 1, 0);
if (test_and_set_bit(buf_nr, &d_out->busy)) {
WARN("ep %d urb %d busy %#lx", EP_D_OUT, buf_nr, d_out->busy);
return;
}
urb = d_out->urb[buf_nr];
DBG_SKB(0x10, skb);
len = isdnhdlc_encode(&d_out->hdlc_state,
skb->data, skb->len, &bytes_sent,
urb->transfer_buffer, 16);
skb_pull(skb, bytes_sent);
if(len < 16)
FsmChangeState(&d_out->fsm, ST_DOUT_SHORT_INIT);
else
FsmChangeState(&d_out->fsm, ST_DOUT_LONG_INIT);
if (skb->len == 0) {
d_out->tx_skb = NULL;
D_L1L2(adapter, PH_DATA | CONFIRM, NULL);
dev_kfree_skb_any(skb);
}
// Prepare the URB
urb->transfer_buffer_length = len;
urb->iso_frame_desc[0].offset = 0;
urb->iso_frame_desc[0].length = len;
urb->number_of_packets = 1;
// Prepare the URB
urb->dev = adapter->usb_dev;
urb->transfer_flags = USB_ISO_ASAP;
DBG_ISO_PACKET(0x20,urb);
SUBMIT_URB(urb);
}
static int raw_recvmsg(struct kiocb *iocb, struct socket *sock,
struct msghdr *msg, size_t size, int flags)
{
struct sock *sk = sock->sk;
struct sk_buff *skb;
int error = 0;
int noblock;
DBG("socket %p, sk %p\n", sock, sk);
noblock = flags & MSG_DONTWAIT;
flags &= ~MSG_DONTWAIT;
skb = skb_recv_datagram(sk, flags, noblock, &error);
if (!skb)
return error;
DBG("delivering skbuff %p\n", skb);
DBG_SKB(skb);
if (size < skb->len)
msg->msg_flags |= MSG_TRUNC;
else
size = skb->len;
error = memcpy_toiovec(msg->msg_iov, skb->data, size);
if (error < 0) {
skb_free_datagram(sk, skb);
return error;
}
sock_recv_timestamp(msg, sk, skb);
if (msg->msg_name) {
msg->msg_namelen = sizeof(struct sockaddr_can);
memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
}
DBG("freeing sock %p, skbuff %p\n", sk, skb);
skb_free_datagram(sk, skb);
return size;
}
static inline void hdlc_rpr_irq(struct fritz_bcs *bcs, u32 stat)
{
struct fritz_adapter *adapter = bcs->adapter;
struct sk_buff *skb;
int len;
if (stat & HDLC_STAT_RDO) {
DBG(0x10, "RDO");
bcs->ctrl.sr.xml = 0;
bcs->ctrl.sr.cmd |= HDLC_CMD_RRS;
adapter->write_ctrl(bcs, 1);
bcs->ctrl.sr.cmd &= ~HDLC_CMD_RRS;
adapter->write_ctrl(bcs, 1);
bcs->rcvidx = 0;
return;
}
len = (stat & HDLC_STAT_RML_MASK) >> 8;
if (len == 0)
len = 32;
hdlc_empty_fifo(bcs, len);
if ((stat & HDLC_STAT_RME) || (bcs->mode == L1_MODE_TRANS)) {
if (((stat & HDLC_STAT_CRCVFRRAB)== HDLC_STAT_CRCVFR) ||
(bcs->mode == L1_MODE_TRANS)) {
skb = dev_alloc_skb(bcs->rcvidx);
if (!skb) {
printk(KERN_WARNING "HDLC: receive out of memory\n");
} else {
memcpy(skb_put(skb, bcs->rcvidx), bcs->rcvbuf,
bcs->rcvidx);
DBG_SKB(1, skb);
B_L1L2(bcs, PH_DATA | INDICATION, skb);
}
bcs->rcvidx = 0;
} else {
DBG(0x10, "ch%d invalid frame %#x",
bcs->channel, stat);
bcs->rcvidx = 0;
}
}
}
/*
* Encode and transmit next frame.
*/
static void usb_b_out(struct st5481_bcs *bcs,int buf_nr)
{
struct st5481_b_out *b_out = &bcs->b_out;
struct st5481_adapter *adapter = bcs->adapter;
struct urb *urb;
unsigned int packet_size,offset;
int len,buf_size,bytes_sent;
int i;
struct sk_buff *skb;
if (test_and_set_bit(buf_nr, &b_out->busy)) {
DBG(4,"ep %d urb %d busy",(bcs->channel+1)*2,buf_nr);
return;
}
urb = b_out->urb[buf_nr];
// Adjust isoc buffer size according to flow state
if(b_out->flow_event & (OUT_DOWN | OUT_UNDERRUN)) {
buf_size = NUM_ISO_PACKETS_B*SIZE_ISO_PACKETS_B_OUT + B_FLOW_ADJUST;
packet_size = SIZE_ISO_PACKETS_B_OUT + B_FLOW_ADJUST;
DBG(4,"B%d,adjust flow,add %d bytes",bcs->channel+1,B_FLOW_ADJUST);
} else if(b_out->flow_event & OUT_UP){
buf_size = NUM_ISO_PACKETS_B*SIZE_ISO_PACKETS_B_OUT - B_FLOW_ADJUST;
packet_size = SIZE_ISO_PACKETS_B_OUT - B_FLOW_ADJUST;
DBG(4,"B%d,adjust flow,remove %d bytes",bcs->channel+1,B_FLOW_ADJUST);
} else {
buf_size = NUM_ISO_PACKETS_B*SIZE_ISO_PACKETS_B_OUT;
packet_size = 8;
}
b_out->flow_event = 0;
len = 0;
while (len < buf_size) {
if ((skb = b_out->tx_skb)) {
DBG_SKB(0x100, skb);
DBG(4,"B%d,len=%d",bcs->channel+1,skb->len);
if (bcs->mode == L1_MODE_TRANS) {
bytes_sent = buf_size - len;
if (skb->len < bytes_sent)
bytes_sent = skb->len;
{ /* swap tx bytes to get hearable audio data */
register unsigned char *src = skb->data;
register unsigned char *dest = urb->transfer_buffer+len;
register unsigned int count;
for (count = 0; count < bytes_sent; count++)
*dest++ = isdnhdlc_bit_rev_tab[*src++];
}
len += bytes_sent;
} else {
len += isdnhdlc_encode(&b_out->hdlc_state,
skb->data, skb->len, &bytes_sent,
urb->transfer_buffer+len, buf_size-len);
}
skb_pull(skb, bytes_sent);
if (!skb->len) {
// Frame sent
b_out->tx_skb = NULL;
B_L1L2(bcs, PH_DATA | CONFIRM, (void *) skb->truesize);
dev_kfree_skb_any(skb);
/* if (!(bcs->tx_skb = skb_dequeue(&bcs->sq))) { */
/* st5481B_sched_event(bcs, B_XMTBUFREADY); */
/* } */
}
} else {
if (bcs->mode == L1_MODE_TRANS) {
memset(urb->transfer_buffer+len, 0xff, buf_size-len);
len = buf_size;
} else {
// Send flags
len += isdnhdlc_encode(&b_out->hdlc_state,
NULL, 0, &bytes_sent,
urb->transfer_buffer+len, buf_size-len);
}
}
}
// Prepare the URB
for (i = 0, offset = 0; offset < len; i++) {
urb->iso_frame_desc[i].offset = offset;
urb->iso_frame_desc[i].length = packet_size;
offset += packet_size;
packet_size = SIZE_ISO_PACKETS_B_OUT;
}
urb->transfer_buffer_length = len;
urb->number_of_packets = i;
urb->dev = adapter->usb_dev;
DBG_ISO_PACKET(0x200,urb);
SUBMIT_URB(urb, GFP_NOIO);
}
