usb-buffer.c
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2010:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include <usb/usb.h>
00022 #include <usb/usb_device.h>
00023 #include "usb_function_cdc.h"
00024 #include "usb-buffer.h"
00025 #include "usb_uart.h"
00026 
00027 struct fifo {
00028         unsigned char * buffer;
00029         size_t size;
00030         size_t insert;
00031         size_t consume;
00032 };
00033 
00034 static struct {
00035         struct fifo rx;
00036         struct fifo tx;
00037 } AsebaUsb;
00038 
00039 /* Basic assumption in order to protect concurrent access to the fifos:
00040         - If the code in "main()" access the fifo it need to disable the usb interrupt 
00041 */
00042 
00043 static inline size_t get_used(struct fifo * f) {
00044         size_t ipos, cpos;
00045         ipos = f->insert;
00046         cpos = f->consume;
00047         if (ipos >= cpos)
00048                 return ipos - cpos;
00049         else
00050                 return f->size - cpos + ipos;
00051 }
00052 
00053 static inline size_t get_free(struct fifo * f) {
00054         return f->size - get_used(f);
00055 }
00056 
00057 /* you MUST ensure that you pass correct size to thoses two function, 
00058  * no check are done .... 
00059  */
00060 static inline void memcpy_out_fifo(unsigned char * dest, struct fifo * f, size_t size) {
00061         while(size--) {
00062                 *dest++ = f->buffer[f->consume++];
00063                 if(f->consume == f->size)
00064                         f->consume = 0;
00065         }
00066 }
00067 
00068 static inline void memcpy_to_fifo(struct fifo * f, const unsigned char * src, size_t size) {
00069         while(size--) {
00070                 f->buffer[f->insert++] = *src++;
00071                 if(f->insert == f->size)
00072                         f->insert = 0;
00073         }
00074 }
00075 
00076 static inline void fifo_peek(unsigned char * d, struct fifo * f,size_t size) {
00077         int ct = f->consume;
00078         while(size--) {
00079                 *d++ = f->buffer[ct++];
00080                 if(ct == f->size)
00081                         ct = 0;
00082         }
00083 }       
00084 
00085 static inline void fifo_reset(struct fifo * f) {
00086         f->insert = f->consume = 0;
00087 }
00088 
00089 /* USB interrupt part */
00090 // They can be called from the main, but with usb interrupt disabled, so it's OK
00091 static int tx_busy;
00092 static int debug;
00093 
00094 unsigned char AsebaTxReady(unsigned char *data) {
00095         size_t size = get_used(&AsebaUsb.tx);
00096         
00097         if(size == 0) {
00098                 tx_busy = 0;
00099                 debug = 0;
00100                 return 0;
00101         }
00102         
00103         if(size > ASEBA_USB_MTU)
00104                 size = ASEBA_USB_MTU;
00105         
00106         memcpy_out_fifo(data, &AsebaUsb.tx, size);
00107         debug ++;
00108         return size;
00109 }
00110 
00111 int AsebaUsbBulkRecv(unsigned char *data, unsigned char size) {
00112         size_t free = get_free(&AsebaUsb.rx);
00113         
00114         if(size >= free)
00115                 return 1;
00116         
00117         memcpy_to_fifo(&AsebaUsb.rx, data, size);
00118         
00119         return 0;
00120 }
00121 
00122 /* main() part */
00123 
00124 void AsebaSendBuffer(AsebaVMState *vm, const uint8 *data, uint16 length) {
00125         int flags;
00126         // Here we must loop until we can send the data.
00127         // BUT if the usb connection is not available, we drop the packet
00128         if(!usb_uart_serial_port_open())
00129                 return;
00130         
00131         // Sanity check, should never be true
00132         if (length < 2)
00133                 return;
00134         
00135         do {
00136                 USBMaskInterrupts(flags);
00137                 if(get_free(&AsebaUsb.tx) > length + 4) {
00138                         length -= 2;
00139                         memcpy_to_fifo(&AsebaUsb.tx, (unsigned char *) &length, 2);
00140                         memcpy_to_fifo(&AsebaUsb.tx, (unsigned char *) &vm->nodeId, 2);
00141                         memcpy_to_fifo(&AsebaUsb.tx, (unsigned char *) data, length + 2);
00142                         
00143                         // Will callback AsebaUsbTxReady
00144                         if (!tx_busy) {
00145                                 tx_busy = 1;
00146                                 USBCDCKickTx();
00147                         }
00148                         
00149                         length = 0;
00150                 }
00151                 
00152         
00153                 
00154                 // Usb can be disconnected while sending ...
00155                 if(!usb_uart_serial_port_open()) {
00156                         fifo_reset(&AsebaUsb.tx);
00157                         USBUnmaskInterrupts(flags);
00158                         break;
00159                 }               
00160                 
00161                 USBUnmaskInterrupts(flags);
00162         } while(length);
00163 }
00164 
00165 uint16 AsebaGetBuffer(AsebaVMState *vm, uint8 * data, uint16 maxLength, uint16* source) {
00166         int flags;
00167         uint16 ret = 0;
00168         size_t u;
00169         // Touching the FIFO, mask the interrupt ...
00170         USBMaskInterrupts(flags);
00171 
00172         u = get_used(&AsebaUsb.rx);
00173 
00174         /* Minium packet size == len + src + msg_type == 6 bytes */
00175         if(u >= 6) {
00176                 int len;
00177                 fifo_peek((unsigned char *) &len, &AsebaUsb.rx, 2);
00178                 if (u >= len + 6) {
00179                         memcpy_out_fifo((unsigned char *) &len, &AsebaUsb.rx, 2);
00180                         memcpy_out_fifo((unsigned char *) source, &AsebaUsb.rx, 2);
00181                         // msg_type is not in the len but is always present
00182                         len = len + 2;
00183                         /* Yay ! We have a complete packet ! */
00184                         if(len > maxLength)
00185                                 len = maxLength;
00186                         memcpy_out_fifo(data, &AsebaUsb.rx, len);
00187                         ret = len;
00188                 }
00189         }
00190         if(usb_uart_serial_port_open())
00191                 USBCDCKickRx();
00192         else    
00193                 fifo_reset(&AsebaUsb.rx);
00194 
00195         USBUnmaskInterrupts(flags);
00196         return ret;
00197 }
00198 
00199 void AsebaUsbInit(unsigned char * sendQueue, size_t sendQueueSize, unsigned char * recvQueue, size_t recvQueueSize) {
00200         AsebaUsb.tx.buffer = sendQueue;
00201         AsebaUsb.tx.size = sendQueueSize;
00202         
00203         AsebaUsb.rx.buffer = recvQueue;
00204         AsebaUsb.rx.size = recvQueueSize;
00205 }
00206 
00207 int AsebaUsbRecvBufferEmpty(void) {
00208         // We are called with interrupt disabled ! Check if rx contain something meaningfull
00209         
00210         int u;
00211         
00212         u = get_used(&AsebaUsb.rx);
00213         if(u > 6) {
00214                 int len;
00215                 fifo_peek((unsigned char *) &len, &AsebaUsb.rx, 2);
00216                 if (u >= len + 6) 
00217                         return 0;
00218         }
00219         return 1;
00220 }
00221 
00222 int AsebaUsbTxBusy(void) {
00223         return tx_busy;
00224 }


aseba
Author(s): Stéphane Magnenat
autogenerated on Sun Oct 5 2014 23:46:39