X-Git-Url: http://rtime.felk.cvut.cz/gitweb/lincan.git/blobdiff_plain/3c6d0376edd06bad63e861415974556259f593cd..f6c113a5fb217e7495b93053cf29fa6966923445:/lincan/src/usbcan.c diff --git a/lincan/src/usbcan.c b/lincan/src/usbcan.c index 91807f7..b356373 100644 --- a/lincan/src/usbcan.c +++ b/lincan/src/usbcan.c @@ -54,8 +54,7 @@ int usbcan_request_io(struct candevice_t *candev) } /* start kernel thread */ - dev->rcvthread.arg = dev; - start_kthread(usbcan_read_kthread, &dev->rcvthread); + dev->comthread=can_kthread_run(usbcan_read_kthread, (void *)dev, "usbcan"); /* Adding link to can device into usbcan_usb struct */ ((struct usbcan_usb*)candev->sysdevptr.anydev)->candev=candev; @@ -77,13 +76,12 @@ int usbcan_request_io(struct candevice_t *candev) int usbcan_release_io(struct candevice_t *candev) { struct usbcan_usb *dev = ((struct usbcan_usb*)candev->sysdevptr.anydev); + if (!dev) + return 0; /* terminate the kernel thread */ - if (dev->rcv){ - usb_kill_urb(dev->rcv); - usb_free_urb(dev->rcv); - } - stop_kthread(&dev->rcvthread); + can_kthread_stop(dev->comthread); + return 0; } @@ -277,6 +275,9 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned l u8 usbbuf[16]; + if (!dev) + return -ENODEV; + *(uint32_t *)(usbbuf)=cpu_to_le32(mask); *(uint32_t *)(usbbuf+4)=cpu_to_le32(code); @@ -284,7 +285,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned l usb_sndctrlpipe(dev->udev, dev->ctl_out_endpointAddr), USBCAN_VENDOR_EXT_MASK_SET, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), &usbbuf, 16, 10000); if (retval<0) @@ -294,7 +295,7 @@ int usbcan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned l usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr), USBCAN_VENDOR_EXT_MASK_STATUS, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), &usbbuf, 16, 10000); @@ -330,6 +331,9 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw, u8 usbbuf[16]; + if (!dev) + return -ENODEV; + *(int32_t *)(usbbuf)=cpu_to_le32(rate); *(int32_t *)(usbbuf+4)=cpu_to_le32(sjw); *(int32_t *)(usbbuf+8)=cpu_to_le32(sampl_pt); @@ -339,7 +343,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw, usb_sndctrlpipe(dev->udev, dev->ctl_out_endpointAddr), USBCAN_VENDOR_BAUD_RATE_SET, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), &usbbuf, 16, 10000); if (retval<0) @@ -349,7 +353,7 @@ int usbcan_baud_rate(struct canchip_t *chip, int rate, int clock, int sjw, usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr), USBCAN_VENDOR_BAUD_RATE_STATUS, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), usbbuf, 16, 10000); @@ -399,6 +403,8 @@ int usbcan_pre_write_config(struct canchip_t *chip, struct msgobj_t *obj, int len; u8 *ptr; + if (!dev) + return -ENODEV; /* Wait until Transmit Buffer Status is released */ while ( usbcan_chip_queue_status(chip) && i++hostdevice->sysdevptr.anydev; int len,retval; + if (!dev) + return -ENODEV; + set_bit(USBCAN_TX_PENDING,&dev->flags); retval=usb_bulk_msg(dev->udev, usb_sndbulkpipe(dev->udev, dev->bulk_out_endpointAddr), @@ -458,7 +467,23 @@ int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj, CANMSG("CAN message not sent\n"); return -EIO; } + CANMSG("Message sent\n"); + + if(obj->tx_slot){ + // Do local transmitted message distribution if enabled + if (processlocal){ + // fill CAN message timestamp + can_filltimestamp(&obj->tx_slot->msg.timestamp); + obj->tx_slot->msg.flags |= MSG_LOCAL; + canque_filter_msg2edges(obj->qends, &obj->tx_slot->msg); + } + // Free transmitted slot + canque_free_outslot(obj->qends, obj->tx_qedge, obj->tx_slot); + obj->tx_slot=NULL; + } + + can_msgobj_clear_fl(obj,TX_PENDING); return 0; } @@ -473,7 +498,10 @@ int usbcan_send_msg(struct canchip_t *chip, struct msgobj_t *obj, */ int usbcan_check_tx_stat(struct canchip_t *chip) { - if (test_bit(USBCAN_TX_PENDING,&((struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev)->flags)) + struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev; + if (!dev) + return 0; + if (test_bit(USBCAN_TX_PENDING,&dev->flags)) return 1; return 0; } @@ -494,11 +522,14 @@ int usbcan_set_btregs(struct canchip_t *chip, unsigned short btr0, struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev; uint16_t value=(btr1&0xFF)<<8 | (btr0&0xFF); + if (!dev) + return -ENODEV; + retval = usb_control_msg(dev->udev, usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr), USBCAN_VENDOR_SET_BTREGS, USB_TYPE_VENDOR, - cpu_to_le16(value), chip->chip_idx, + cpu_to_le16(value), cpu_to_le16(chip->chip_idx), dev->ctl_in_buffer, dev->ctl_in_size, 10000); @@ -521,11 +552,14 @@ int usbcan_start_chip(struct canchip_t *chip) int retval; struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev; + if (!dev) + return -ENODEV; + retval = usb_control_msg(dev->udev, usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr), USBCAN_VENDOR_START_CHIP, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), dev->ctl_in_buffer, dev->ctl_in_size, 10000); @@ -547,23 +581,29 @@ int usbcan_start_chip(struct canchip_t *chip) */ int usbcan_chip_queue_status(struct canchip_t *chip) { - int retval; + int retval,i; struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev; + if (!dev) + return -ENODEV; retval = usb_control_msg(dev->udev, usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr), USBCAN_VENDOR_CHECK_TX_STAT, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), dev->ctl_in_buffer, dev->ctl_in_size, 10000); + for (i=0;ictl_in_size;i++) + CANMSG("Buffer content: %d\n",dev->ctl_in_buffer[i]); if (retval==1){ + CANMSG("Chip_queue_status: %d\n",dev->ctl_in_buffer[0]); if(dev->ctl_in_buffer[0]==1) return 0; if(dev->ctl_in_buffer[0]==0) return 1; } + CANMSG("Chip_queue_status error: %d\n",retval); return -ENODEV; } @@ -579,11 +619,14 @@ int usbcan_stop_chip(struct canchip_t *chip) int retval; struct usbcan_usb *dev=(struct usbcan_usb*)chip->hostdevice->sysdevptr.anydev; + if (!dev) + return -ENODEV; + retval = usb_control_msg(dev->udev, usb_rcvctrlpipe(dev->udev, dev->ctl_in_endpointAddr), USBCAN_VENDOR_STOP_CHIP, USB_TYPE_VENDOR, - 0, chip->chip_idx, + cpu_to_le16(0), cpu_to_le16(chip->chip_idx), dev->ctl_in_buffer, dev->ctl_in_size, 10000); @@ -848,7 +891,7 @@ int usbcan_irq_handler(int irq, struct canchip_t *chip) */ int usbcan_wakeup_tx(struct canchip_t *chip, struct msgobj_t *obj) { - + CANMSG("Trying to send message\n"); can_preempt_disable(); can_msgobj_set_fl(obj,TX_PENDING); @@ -962,6 +1005,30 @@ int usbcan_init_chip_data(struct candevice_t *candev, int chipnr) /* --------------------------------------------------------------------------------------------------- */ +static int usbcan_sleep_thread(struct usbcan_usb *dev) +{ + int rc = 0; + + /* Wait until a signal arrives or we are woken up */ + for (;;) { + try_to_freeze(); + set_current_state(TASK_INTERRUPTIBLE); + if (signal_pending(current)) { + rc = -EINTR; + break; + } + if ( + can_kthread_should_stop() || + test_bit(USBCAN_DATA_READ,&dev->flags) || + test_bit(USBCAN_TERMINATE,&dev->flags) || + test_bit(USBCAN_ERROR,&dev->flags) + ) + break; + schedule(); + } + __set_current_state(TASK_RUNNING); + return rc; +} static void usbcan_rcv(struct urb *urb) { @@ -972,7 +1039,8 @@ static void usbcan_rcv(struct urb *urb) case 0: /* success */ set_bit(USBCAN_DATA_READ,&dev->flags); - wake_up(&dev->rcvthread.queue); + CANMSG("Message received\n"); + wake_up_process(dev->comthread); return; case -ECONNRESET: case -ENOENT: @@ -980,10 +1048,10 @@ static void usbcan_rcv(struct urb *urb) /* this urb is terminated, clean up */ CANMSG("%s - urb shutting down with status: %d\n", __FUNCTION__, urb->status); set_bit(USBCAN_TERMINATE,&dev->flags); - wake_up(&dev->rcvthread.queue); + wake_up_process(dev->comthread); return; default: - CANMSG("%s - nonzero urb status received: %d\n", __FUNCTION__, urb->status); +// CANMSG("%s - nonzero urb status received: %d\n", __FUNCTION__, urb->status); break; } @@ -992,54 +1060,50 @@ static void usbcan_rcv(struct urb *urb) CANMSG("%s - usb_submit_urb failed with result %d\n", __FUNCTION__, retval); set_bit(USBCAN_ERROR,&dev->flags); - wake_up(&dev->rcvthread.queue); + wake_up_process(dev->comthread); } } -void usbcan_read_kthread(kthread_t *kthread) +int usbcan_read_kthread(void *data) { int retval; - struct usbcan_usb *dev=(struct usbcan_usb *)kthread->arg; + struct usbcan_usb *dev=(struct usbcan_usb *)data; struct msgobj_t *obj; - /* setup the thread environment */ - init_kthread(kthread, "usbcan"); - /* this is normal work to do */ CANMSG ("usbcan thread started!\n"); - dev->rcv = usb_alloc_urb(0, GFP_KERNEL); - if (!dev->rcv){ + dev->rx = usb_alloc_urb(0, GFP_KERNEL); + if (!dev->rx){ CANMSG("Error allocating usb urb\n"); goto error; } - dev->rcv->dev = dev->udev; - usb_fill_bulk_urb(dev->rcv, dev->udev, + dev->rx->dev = dev->udev; + usb_fill_bulk_urb(dev->rx, dev->udev, usb_rcvbulkpipe(dev->udev, dev->bulk_in_endpointAddr), - &dev->rcv_msg, 16, + &dev->rx_msg, 16, usbcan_rcv, dev); /* an endless loop in which we are doing our work */ for(;;) { - retval=usb_submit_urb(dev->rcv, GFP_KERNEL); - if (retval){ - CANMSG("URB error %d\n",retval); - break; - } - /* fall asleep */ - wait_event_interruptible(kthread->queue, - test_bit(USBCAN_DATA_READ,&dev->flags) - || test_bit(USBCAN_TERMINATE,&dev->flags) - || test_bit(USBCAN_ERROR,&dev->flags) - ); + if (!can_kthread_should_stop()){ + retval=usb_submit_urb(dev->rx, GFP_KERNEL); + if (retval){ + CANMSG("URB error %d\n",retval); + break; + } + + /* fall asleep */ + usbcan_sleep_thread(dev); + } /* We need to do a memory barrier here to be sure that the flags are visible on all CPUs. */ mb(); /* here we are back from sleep because we caught a signal. */ - if (kthread->terminate) + if (can_kthread_should_stop()) { /* we received a request to terminate ourself */ break; @@ -1054,22 +1118,23 @@ void usbcan_read_kthread(kthread_t *kthread) if (test_bit(USBCAN_DATA_READ,&dev->flags)){ int i, len; clear_bit(USBCAN_DATA_READ,&dev->flags); + CANMSG("Thread got received message\n"); - if ((dev->candev->chip[dev->rcv_msg[0]])&& - (dev->candev->chip[dev->rcv_msg[0]]->flags & CHIP_CONFIGURED) + if ((dev->candev->chip[dev->rx_msg[0]])&& + (dev->candev->chip[dev->rx_msg[0]]->flags & CHIP_CONFIGURED) ){ u8 *ptr; - obj=dev->candev->chip[dev->rcv_msg[0]]->msgobj[0]; + obj=dev->candev->chip[dev->rx_msg[0]]->msgobj[0]; - len=*(uint8_t *)(dev->rcv_msg+1); + len=*(uint8_t *)(dev->rx_msg+1); if(len > CAN_MSG_LENGTH) len = CAN_MSG_LENGTH; obj->rx_msg.length = len; - obj->rx_msg.flags=le16_to_cpu(*(uint16_t *)(dev->rcv_msg+2)); - obj->rx_msg.id=le32_to_cpu((*(uint32_t *)(dev->rcv_msg+4))); + obj->rx_msg.flags=le16_to_cpu(*(uint16_t *)(dev->rx_msg+2)); + obj->rx_msg.id=le32_to_cpu((*(uint32_t *)(dev->rx_msg+4))); - for(ptr=dev->rcv_msg+8,i=0; i < len; ptr++,i++) { + for(ptr=dev->rx_msg+8,i=0; i < len; ptr++,i++) { obj->rx_msg.data[i]=*ptr; } @@ -1077,14 +1142,22 @@ void usbcan_read_kthread(kthread_t *kthread) can_filltimestamp(&obj->rx_msg.timestamp); canque_filter_msg2edges(obj->qends, &obj->rx_msg); } + else + CANMSG("Destination chip not found\n"); } } } /* here we go only in case of termination of the thread */ + if (dev->rx){ + usb_kill_urb(dev->rx); + usb_free_urb(dev->rx); + } + return 0; error: /* cleanup the thread, leave */ CANMSG ("kernel thread terminated!\n"); - exit_kthread(kthread); + return -ENOMEM; +// exit_kthread(kthread); /* returning from the thread here calls the exit functions */ } @@ -1107,6 +1180,7 @@ static int usbcan_probe(struct usb_interface *interface, const struct usb_device memset(dev, 0, sizeof(struct usbcan_usb)); sema_init(&dev->limit_sem, WRITES_IN_FLIGHT); + mutex_init(&dev->io_mutex); spin_lock_init(&dev->err_lock); init_usb_anchor(&dev->submitted); @@ -1139,31 +1213,15 @@ static int usbcan_probe(struct usb_interface *interface, const struct usb_device dev->bulk_out_endpointAddr = endpoint->bEndpointAddress; } - if (!dev->ctl_in_endpointAddr && - usb_endpoint_xfer_control(endpoint) && - usb_endpoint_dir_in(endpoint)) { - /* we found a bulk in endpoint */ - buffer_size = le16_to_cpu(endpoint->wMaxPacketSize); - dev->ctl_in_size = buffer_size; - dev->ctl_in_endpointAddr = endpoint->bEndpointAddress; - dev->ctl_in_buffer = can_checked_malloc(buffer_size); - if (!dev->ctl_in_buffer) { - err("Could not allocate bulk_in_buffer"); - goto error; - } - } - - if (!dev->ctl_out_endpointAddr && - usb_endpoint_xfer_control(endpoint) && - usb_endpoint_dir_out(endpoint)) { - /* we found a bulk out endpoint */ - dev->ctl_out_endpointAddr = endpoint->bEndpointAddress; - } } if (!(dev->bulk_in_endpointAddr && dev->bulk_out_endpointAddr)) { err("Could not find all bulk-in and bulk-out endpoints"); goto error; } + dev->ctl_in_endpointAddr=0; + dev->ctl_in_size=16; + dev->ctl_in_buffer = can_checked_malloc(dev->ctl_in_size); + dev->ctl_out_endpointAddr=0; /* save our data pointer in this interface device */ usb_set_intfdata(interface, dev); @@ -1207,14 +1265,17 @@ static void usbcan_disconnect(struct usb_interface *interface) //usb_kill_anchored_urbs(&dev->submitted); usb_put_dev(dev->udev); - if (dev->bulk_in_buffer) - can_checked_free(dev->bulk_in_buffer); - if (dev->ctl_in_buffer) - can_checked_free(dev->ctl_in_buffer); + if (dev->candev){ dev->candev->sysdevptr.anydev=NULL; cleanup_usbdev(dev->candev); } + + if (dev->bulk_in_buffer) + can_checked_free(dev->bulk_in_buffer); + if (dev->ctl_in_buffer) + can_checked_free(dev->ctl_in_buffer); + can_checked_free(dev); info("USB Skeleton now disconnected");