can-net.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 "can-net.h"
00022 #include <stdlib.h>
00023 #include <string.h>
00024 
00025 #define TYPE_SMALL_PACKET 0x3
00026 #define TYPE_PACKET_NORMAL 0x0
00027 #define TYPE_PACKET_START 0x1
00028 #define TYPE_PACKET_STOP 0x2
00029 
00030 #define CANID_TO_TYPE(canid) ((canid) >> 8)
00031 #define CANID_TO_ID(canid) ((canid) & 0xff)
00032 #define TO_CANID(type, id) (((type) << 8) | (id))
00033 
00034 #define ASEBA_MIN(a, b) (((a) < (b)) ? (a) : (b))
00035 
00036 
00038 static struct AsebaCan
00039 {
00040         // data
00041         uint16 id; 
00043         // pointer to physical layer functions
00044         AsebaCanSendFrameFP sendFrameFP;
00045         AsebaCanIntVoidFP isFrameRoomFP;
00046         AsebaCanVoidVoidFP receivedPacketDroppedFP;
00047         AsebaCanVoidVoidFP sentPacketDroppedFP;
00048         
00049         // send buffer
00050         CanFrame* sendQueue;
00051         size_t sendQueueSize;
00052         uint16 sendQueueInsertPos;
00053         uint16 sendQueueConsumePos;
00054         
00055         // reception buffer
00056         CanFrame* recvQueue;
00057         size_t recvQueueSize;
00058         uint16 recvQueueInsertPos;
00059         uint16 recvQueueConsumePos;
00060 
00061         uint16 volatile sendQueueLock;
00062         
00063 } asebaCan;
00064 
00067 
00069 uint16 AsebaCanGetMinMultipleOfHeight(uint16 v)
00070 {
00071         uint16 m = v >> 3;
00072         if ((v & 0x7) != 0)
00073                 m++;
00074         return m;
00075 }
00076 
00078 static uint16 AsebaCanSendQueueGetUsedFrames()
00079 {
00080         uint16 ipos, cpos;
00081         ipos = asebaCan.sendQueueInsertPos;
00082         cpos =  asebaCan.sendQueueConsumePos;
00083         
00084         if (ipos >= cpos)
00085                 return ipos - cpos;
00086         else
00087                 return asebaCan.sendQueueSize - cpos + ipos;
00088 }
00089 
00091 static uint16 AsebaCanSendQueueGetFreeFrames()
00092 {
00093         return asebaCan.sendQueueSize - AsebaCanSendQueueGetUsedFrames();
00094 }
00095 
00097 static void AsebaCanSendQueueInsert(uint16 canid, const uint8 *data, size_t size)
00098 {
00099         uint16 temp;
00100         memcpy(asebaCan.sendQueue[asebaCan.sendQueueInsertPos].data, data, size);
00101         asebaCan.sendQueue[asebaCan.sendQueueInsertPos].id = canid;
00102         asebaCan.sendQueue[asebaCan.sendQueueInsertPos].len = size;
00103 
00104         
00105         temp = asebaCan.sendQueueInsertPos + 1;
00106         if (temp >= asebaCan.sendQueueSize)
00107                 temp = 0;
00108         asebaCan.sendQueueInsertPos = temp;
00109 }
00110 
00112 static void AsebaCanSendQueueToPhysicalLayer()
00113 {
00114         uint16 temp;
00115         
00116         while (asebaCan.isFrameRoomFP() && (asebaCan.sendQueueConsumePos != asebaCan.sendQueueInsertPos))
00117         {
00118                 asebaCan.sendQueueLock = 1;
00119                 if(!(asebaCan.isFrameRoomFP() && (asebaCan.sendQueueConsumePos != asebaCan.sendQueueInsertPos))) 
00120                 {
00121                         asebaCan.sendQueueLock = 0;
00122                         continue;
00123                 }
00124                 
00125                 asebaCan.sendFrameFP(asebaCan.sendQueue + asebaCan.sendQueueConsumePos);
00126                 
00127                 temp = asebaCan.sendQueueConsumePos + 1;
00128                 if (temp >= asebaCan.sendQueueSize)
00129                         temp = 0;
00130                 asebaCan.sendQueueConsumePos = temp; 
00131                 asebaCan.sendQueueLock = 0;
00132         }
00133 
00134 }
00135 
00137 static uint16 AsebaCanRecvQueueGetMaxUsedFrames()
00138 {
00139         uint16 ipos, cpos;
00140         ipos = asebaCan.recvQueueInsertPos;
00141         cpos = asebaCan.recvQueueConsumePos;
00142         if (ipos >= cpos)
00143                 return ipos - cpos;
00144         else
00145                 return asebaCan.recvQueueSize - cpos + ipos;
00146 }
00147 
00149 static uint16 AsebaCanRecvQueueGetMinFreeFrames()
00150 {
00151         return asebaCan.recvQueueSize - AsebaCanRecvQueueGetMaxUsedFrames();
00152 }
00153 
00155 static void AsebaCanRecvQueueGarbageCollect()
00156 {
00157         uint16 temp;
00158         while (asebaCan.recvQueueConsumePos != asebaCan.recvQueueInsertPos)
00159         {
00160                 if (asebaCan.recvQueue[asebaCan.recvQueueConsumePos].used)
00161                         break;
00162                 
00163                 temp = asebaCan.recvQueueConsumePos + 1;
00164                 if (temp >= asebaCan.recvQueueSize)
00165                         temp = 0;
00166                 asebaCan.recvQueueConsumePos = temp;
00167         }
00168 }
00169 
00171 static void AsebaCanRecvQueueFreeFrames(uint16 id)
00172 {
00173         uint16 i;
00174         
00175         // for all used frames...
00176         for (i = asebaCan.recvQueueConsumePos; i != asebaCan.recvQueueInsertPos;)
00177         {
00178                 // if frame is of a specific id, mark frame as unused
00179                 if (CANID_TO_ID(asebaCan.recvQueue[i].id) == id)
00180                         asebaCan.recvQueue[i].used = 0;
00181                 
00182                 i++;
00183                 if (i >= asebaCan.recvQueueSize)
00184                         i = 0;
00185         }
00186 }
00187 
00188 
00189 void AsebaCanInit(uint16 id, AsebaCanSendFrameFP sendFrameFP, AsebaCanIntVoidFP isFrameRoomFP, AsebaCanVoidVoidFP receivedPacketDroppedFP, AsebaCanVoidVoidFP sentPacketDroppedFP, CanFrame* sendQueue, size_t sendQueueSize, CanFrame* recvQueue, size_t recvQueueSize)
00190 {
00191         asebaCan.id = id;
00192         
00193         asebaCan.sendFrameFP = sendFrameFP;
00194         asebaCan.isFrameRoomFP= isFrameRoomFP;
00195         asebaCan.receivedPacketDroppedFP = receivedPacketDroppedFP;
00196         asebaCan.sentPacketDroppedFP = sentPacketDroppedFP;
00197         
00198         asebaCan.sendQueue = sendQueue;
00199         asebaCan.sendQueueSize = sendQueueSize;
00200         asebaCan.sendQueueInsertPos = 0;
00201         asebaCan.sendQueueConsumePos = 0;
00202         
00203         asebaCan.recvQueue = recvQueue;
00204         asebaCan.recvQueueSize = recvQueueSize;
00205         asebaCan.recvQueueInsertPos = 0;
00206         asebaCan.recvQueueConsumePos = 0;
00207         
00208         asebaCan.sendQueueLock = 0;
00209 }
00210 
00211 uint16 AsebaCanSend(const uint8 *data, size_t size)
00212 {
00213         return AsebaCanSendSpecificSource(data, size, asebaCan.id);
00214 }
00215 
00216 uint16 AsebaCanSendSpecificSource(const uint8 *data, size_t size, uint16 source)
00217 {
00218         // send everything we can to maximize the space in the buffer
00219         AsebaCanSendQueueToPhysicalLayer();
00220         
00221         // check free space
00222         uint16 frameRequired = AsebaCanGetMinMultipleOfHeight(size);
00223         if (AsebaCanSendQueueGetFreeFrames() <= frameRequired)
00224         {
00225                 asebaCan.sentPacketDroppedFP();
00226                 return 0;
00227         }
00228                 
00229         // insert
00230         if (size <= 8)
00231         {
00232                 AsebaCanSendQueueInsert(TO_CANID(TYPE_SMALL_PACKET, source), data, size);
00233         }
00234         else
00235         {
00236                 size_t pos = 8;
00237                 
00238                 AsebaCanSendQueueInsert(TO_CANID(TYPE_PACKET_START, source), data, 8);
00239                 while (pos + 8 < size)
00240                 {
00241                         AsebaCanSendQueueInsert(TO_CANID(TYPE_PACKET_NORMAL, source), data + pos, 8);
00242                         pos += 8;
00243                 }
00244                 AsebaCanSendQueueInsert(TO_CANID(TYPE_PACKET_STOP, source), data + pos, size - pos);
00245         }
00246         
00247         // send everything we can to minimize the transmission delay
00248         AsebaCanSendQueueToPhysicalLayer();
00249         
00250         return 1;
00251 }
00252 
00253 
00254 uint16 AsebaCanRecvBufferEmpty(void) 
00255 {
00256         return asebaCan.recvQueueInsertPos == asebaCan.recvQueueConsumePos;
00257 }
00258 
00259 void AsebaCanFrameSent()
00260 {
00261         // send everything we can if we are currently not sending
00262         if (!asebaCan.sendQueueLock)
00263                 AsebaCanSendQueueToPhysicalLayer();
00264 }
00265 
00266 void AsebaCanFlushQueue(void)
00267 {
00268         while(asebaCan.sendQueueConsumePos != asebaCan.sendQueueInsertPos || !asebaCan.isFrameRoomFP())
00269                 AsebaIdle();
00270 }
00271 
00272 uint16 AsebaCanRecv(uint8 *data, size_t size, uint16 *source)
00273 {
00274         int stopPos = -1;
00275         uint16 stopId;
00276         uint16 pos = 0;
00277         uint16 i;
00278         
00279         // first scan for "short packets" and "stops"
00280         for (i = asebaCan.recvQueueConsumePos; i != asebaCan.recvQueueInsertPos; )
00281         {
00282                 if(asebaCan.recvQueue[i].used) {
00283                         
00284                         if (CANID_TO_TYPE(asebaCan.recvQueue[i].id) == TYPE_SMALL_PACKET)
00285                         {
00286                                 // copy data
00287                                 uint16 len = ASEBA_MIN(size, asebaCan.recvQueue[i].len);
00288                                 memcpy(data, asebaCan.recvQueue[i].data, len);
00289                                 *source = CANID_TO_ID(asebaCan.recvQueue[i].id);
00290                                 asebaCan.recvQueue[i].used = 0;
00291                                 
00292                                 // garbage collect
00293                                 AsebaCanRecvQueueGarbageCollect();
00294                                 
00295                                 return len;
00296                         }
00297                         // stop found, we thus must have seen start before
00298                         else if (CANID_TO_TYPE(asebaCan.recvQueue[i].id) == TYPE_PACKET_STOP)
00299                         {
00300                                 stopPos = i;
00301                                 stopId = CANID_TO_ID(asebaCan.recvQueue[i].id);
00302                                 break;
00303                         }
00304                 }
00305                 i++;
00306                 if (i >= asebaCan.recvQueueSize)
00307                         i = 0;
00308         }
00309         
00310         // if we haven't found anything
00311         if (stopPos < 0)
00312                 return 0;
00313         
00314         // collect data
00315         *source = CANID_TO_ID(stopId);
00316         i = asebaCan.recvQueueConsumePos;
00317         while (1)
00318         {
00319                 if(asebaCan.recvQueue[i].used) {
00320 
00321                         if (CANID_TO_ID(asebaCan.recvQueue[i].id) == stopId)
00322                         {
00323                         if (pos < size)
00324                                         {
00325                                         uint16 amount = ASEBA_MIN(asebaCan.recvQueue[i].len, size - pos);
00326                                         memcpy(data + pos, asebaCan.recvQueue[i].data, amount);
00327                                         pos += amount;
00328                                 }
00329                                 asebaCan.recvQueue[i].used = 0;
00330                         }
00331                         
00332                         if (i == stopPos)
00333                                 break;
00334         
00335                 }
00336                 i++;
00337                 if (i >= asebaCan.recvQueueSize)
00338                         i = 0;
00339         }
00340 
00341         // garbage collect
00342         AsebaCanRecvQueueGarbageCollect();
00343         
00344         return pos;
00345 }
00346 
00347 #define MAX_DROPPING_SOURCE 20
00348 static uint16 dropping[MAX_DROPPING_SOURCE];
00349 
00350 void AsebaCanFrameReceived(const CanFrame *frame)
00351 {
00352         
00353         uint16 source = CANID_TO_ID(frame->id);
00354         
00355         // check whether this packet should be filtered or not
00356         if (CANID_TO_TYPE(frame->id) == TYPE_SMALL_PACKET)
00357         {
00358                 if (AsebaShouldDropPacket(source, frame->data))
00359                         return;
00360         }
00361         else if (CANID_TO_TYPE(frame->id) == TYPE_PACKET_START)
00362         {
00363                 if (AsebaShouldDropPacket(source, frame->data))
00364                 {
00365                         uint16 i;
00366                         for (i = 0; i < MAX_DROPPING_SOURCE; i++)
00367                         {
00368                                 if (dropping[i] == 0)
00369                                 {
00370                                         dropping[i] = source + 1;
00371                                         return;
00372                                 }
00373                         }
00374                 }
00375         }
00376         else
00377         {
00378                 uint16 i;
00379                 for (i = 0; i < MAX_DROPPING_SOURCE; i++)
00380                 {
00381                         if (dropping[i] == source + 1)
00382                         {
00383                                 if (CANID_TO_TYPE(frame->id) == TYPE_PACKET_STOP)
00384                                         dropping[i] = 0;
00385                                 return;
00386                         }
00387                 }
00388         }
00389         
00390         if (AsebaCanRecvQueueGetMinFreeFrames() <= 1)
00391         {
00392                 // if packet is stop, free associated frames, otherwise this could lead to everlasting used frames
00393                 if (CANID_TO_TYPE(frame->id) == TYPE_PACKET_STOP) {
00394                         AsebaCanRecvQueueFreeFrames(source);
00395                         AsebaCanRecvQueueGarbageCollect();
00396                 }
00397                 
00398                 // notify user
00399                 asebaCan.receivedPacketDroppedFP();
00400         }
00401         else
00402         {
00403                 uint16 temp;
00404                 // store and increment pos
00405                 memcpy(&asebaCan.recvQueue[asebaCan.recvQueueInsertPos], frame, sizeof(*frame));
00406                 asebaCan.recvQueue[asebaCan.recvQueueInsertPos].used = 1;
00407                 temp = asebaCan.recvQueueInsertPos + 1;
00408                 if (temp >= asebaCan.recvQueueSize)
00409                         temp = 0;
00410                 asebaCan.recvQueueInsertPos = temp;
00411         }
00412 }
00413 
00414 void AsebaCanRecvFreeQueue(void)
00415 {
00416         int i;
00417         for(i = 0; i < asebaCan.recvQueueSize; i++)
00418                 asebaCan.recvQueue[i].used = 0;
00419         asebaCan.recvQueueInsertPos = 0;
00420         asebaCan.recvQueueConsumePos = 0;
00421         
00422         for(i = 0; i < MAX_DROPPING_SOURCE; i++) 
00423                 dropping[i] = 0;
00424 }
00425 


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