1 /***************************************************************************** 2 3 Filename: mfifo.c 4 5 Description: mfifo is a message orriented fifo system with support of 6 both message and byte per byte retriaval of messages. 7 8 The fifo has been designed with two usages in mind: 9 10 - Queueing of frames for hdlc and feeding out byte per byte 11 with the possibility of re-sending of frames etc. 12 13 - fifo for messages of dynamic size. 14 15 The fifo is allocated on top of any buffer and creates an 16 index of message in the queue. The user can write/read 17 messages or write messages and read the message one byte 18 at the time. 19 20 Interface: 21 MFIFOCreate Create/reset/initialize fifo. 22 MFIFOClear Clear FIFO. 23 MFIFOWriteMes Write message into fifo 24 * MFIFOReadMes Read message from fifo. 25 MFIFOGetMesPtr Get ptr to next message. 26 MFIFOKillNext Kill next message. 27 28 * currently not implemented. 29 30 Note: The message will always be saved continuously. If there is not 31 sufficient space at the end of the buffer, the fifo will skip 32 the last bytes and save the message at the top of the buffer. 33 34 This is required to allow direct ptr access to messages 35 stored in the queue. 36 37 License/Copyright: 38 39 Copyright (c) 2007, Jan Vidar Berger, Case Labs, Ltd. All rights reserved. 40 email:janvb@caselaboratories.com 41 42 Redistribution and use in source and binary forms, with or without 43 modification, are permitted provided that the following conditions are 44 met: 45 46 * Redistributions of source code must retain the above copyright notice, 47 this list of conditions and the following disclaimer. 48 * Redistributions in binary form must reproduce the above copyright notice, 49 this list of conditions and the following disclaimer in the documentation 50 and/or other materials provided with the distribution. 51 * Neither the name of the Case Labs, Ltd nor the names of its contributors 52 may be used to endorse or promote products derived from this software 53 without specific prior written permission. 54 55 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 56 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 57 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 58 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 59 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 60 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 61 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 62 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 63 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 64 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 65 POSSIBILITY OF SUCH DAMAGE. 66 *****************************************************************************/ 67 68 #include "mfifo.h" 69 #include <memory.h> 70 #include <stdlib.h> 71 72 /***************************************************************************** 73 74 Function: MFIFOCreate 75 76 Description: Creates a fifo on top of an existing buffer. 77 78 Parameters: buf ptr to buffer. 79 size size of buffer in bytes. 80 index size of index entries (max no messages). 81 82 Return value: 0 if failure, 1 if ok. 83 84 *****************************************************************************/ 85 int MFIFOCreate(unsigned char *buf, int size, int index) 86 { 87 MFIFO *mf = (MFIFO *)buf; 88 89 mf->first = mf->last = 0; 90 mf->ixsize = index; 91 mf->buf = &buf[sizeof(MFIFO) + (sizeof(MINDEX) * index)]; 92 93 if (mf->buf > &buf[size]) 94 return 0; 95 96 mf->bsize = size - sizeof(MFIFO) - (sizeof(MINDEX) * index); 97 98 return 1; 99 } 100 101 /***************************************************************************** 102 103 Function: MFIFOClear 104 105 Description: Clear the FIFO 106 107 Paremeters: buf ptr to fifo 108 109 Return Value: none 110 111 *****************************************************************************/ 112 void MFIFOClear(unsigned char * buf) 113 { 114 MFIFO *mf = (MFIFO *)buf; 115 116 mf->first = mf->last = 0; 117 } 118 119 /***************************************************************************** 120 121 Function: MFIFOGetLBOffset 122 123 Description: Helper function caclulating offset to the 'first out' byte. 124 125 Paremeters: buf ptr to fifo 126 127 Return Value: offset. 128 129 *****************************************************************************/ 130 int MFIFOGetLBOffset(unsigned char *buf) 131 { 132 MFIFO *mf = (MFIFO *)buf; 133 134 if (mf->last != mf->first) 135 return mf->ix[mf->last].offset; 136 137 return 0; 138 } 139 140 /***************************************************************************** 141 142 Function: MFIFOGetFBOffset 143 144 Description: Helper function calculating the offset to the 'first in' 145 byte in the buffer. This is the position the next byte 146 entering the fifo will occupy. 147 148 Paremeters: buf ptr to fifo 149 150 Return Value: offset 151 152 *****************************************************************************/ 153 int MFIFOGetFBOffset(unsigned char *buf) 154 { 155 MFIFO *mf = (MFIFO *)buf; 156 int x; 157 158 if (mf->last == mf->first) 159 return 0; 160 161 x = mf->first - 1; 162 163 if (x < 0) 164 x = mf->ixsize - 1; 165 166 return mf->ix[x].offset + mf->ix[x].size; 167 } 168 169 /***************************************************************************** 170 171 Function: MFIFOWriteIX 172 173 Description: Helper function writing a calculated entry. The function 174 will perform a memcpy to move the message and set the index 175 values as well as increase the 'first in' index. 176 177 Paremeters: buf ptr to fifo 178 mes ptr to message 179 size size of message in bytes. 180 ix index to index entry. 181 off offset to position to receive the message 182 183 Return Value: none 184 185 *****************************************************************************/ 186 void MFIFOWriteIX(unsigned char *buf, unsigned char *mes, int size, int ix, int off) 187 { 188 MFIFO *mf = (MFIFO *)buf; 189 int x; 190 191 memcpy(&mf->buf[off], mes, size); 192 mf->ix[ix].offset = off; 193 mf->ix[ix].size = size; 194 195 x = mf->first + 1; 196 197 if (x >= mf->ixsize) 198 x = 0; 199 200 mf->first = x; 201 } 202 203 /***************************************************************************** 204 205 Function: MFIFOWriteMes 206 207 Description: 208 209 Paremeters: 210 211 Return Value: 212 213 *****************************************************************************/ 214 int MFIFOWriteMes(unsigned char *buf, unsigned char *mes, int size) 215 { 216 MFIFO *mf = (MFIFO *)buf; 217 int of, ol, x; 218 219 x = mf->first + 1; 220 221 if (x >= mf->ixsize) 222 x = 0; 223 224 if (x == mf->last) 225 return 0; /* full queue */ 226 227 of = MFIFOGetFBOffset(buf); 228 ol = MFIFOGetLBOffset(buf); 229 if (mf->last == mf->first) { /* empty queue */ 230 mf->first = mf->last = 0; /* optimize */ 231 232 MFIFOWriteIX(buf, mes, size, mf->first, 0); 233 return 1; 234 } 235 else if (of > ol) { 236 if (mf->bsize - of >= size) { 237 MFIFOWriteIX(buf, mes, size, mf->first, of); 238 return 1; 239 } 240 else if (ol > size) { 241 MFIFOWriteIX(buf, mes, size, mf->first, ol); 242 return 1; 243 } 244 } 245 else if (ol - of > size) { 246 MFIFOWriteIX(buf, mes, size, mf->first, of); 247 return 1; 248 } 249 250 return 0; 251 } 252 253 /***************************************************************************** 254 255 Function: MFIFOGetMesPtr 256 257 Description: 258 259 Paremeters: 260 261 Return Value: 262 263 *****************************************************************************/ 264 unsigned char * MFIFOGetMesPtr(unsigned char *buf, int *size) 265 { 266 MFIFO *mf = (MFIFO *)buf; 267 268 if (mf->first == mf->last) { 269 return NULL; 270 } 271 272 *size = mf->ix[mf->last].size; 273 return &mf->buf[mf->ix[mf->last].offset]; 274 } 275 276 /***************************************************************************** 277 278 Function: MFIFOKillNext 279 280 Description: 281 282 Paremeters: 283 284 Return Value: 285 286 *****************************************************************************/ 287 void MFIFOKillNext(unsigned char *buf) 288 { 289 MFIFO *mf = (MFIFO *)buf; 290 int x; 291 292 if (mf->first != mf->last) { 293 x = mf->last + 1; 294 if (x >= mf->ixsize) { 295 x = 0; 296 } 297 298 mf->last = x; 299 } 300 } 301 302 303 /* 304 * Queue-style accessor functions 305 */ 306 307 /** 308 * MFIFOGetMesPtrOffset 309 * \brief Get pointer to and size of message at position x 310 */ 311 unsigned char * MFIFOGetMesPtrOffset(unsigned char *buf, int *size, const int pos) 312 { 313 MFIFO *mf = (MFIFO *)buf; 314 int x; 315 316 if (mf->first == mf->last) { 317 return NULL; 318 } 319 320 if (pos < 0 || pos >= mf->ixsize) { 321 return NULL; 322 } 323 324 x = pos - mf->last; 325 if (x < 0) { 326 x += (mf->ixsize - 1); 327 } 328 329 *size = mf->ix[x].size; 330 return &mf->buf[mf->ix[x].offset]; 331 } 332 333 334 /** 335 * MFIFOGetMesCount 336 * \brief How many messages are currently in the buffer? 337 */ 338 int MFIFOGetMesCount(unsigned char *buf) 339 { 340 MFIFO *mf = (MFIFO *)buf; 341 342 if (mf->first == mf->last) { 343 return 0; 344 } 345 else if (mf->first > mf->last) { 346 return mf->first - mf->last; 347 } 348 else { 349 return (mf->ixsize - mf->last) + mf->first; 350 } 351 } 352 353 /** 354 * MFIFOWriteMesOverwrite 355 * \brief Same as MFIFOWriteMes but old frames will be overwritten if the fifo is full 356 */ 357 int MFIFOWriteMesOverwrite(unsigned char *buf, unsigned char *mes, int size) 358 { 359 MFIFO *mf = (MFIFO *)buf; 360 int of, ol, x; 361 362 x = mf->first + 1; 363 364 if (x >= mf->ixsize) 365 x = 0; 366 367 if (x == mf->last) { 368 /* advance last pointer */ 369 mf->last++; 370 371 if (mf->last >= mf->ixsize) 372 mf->last = 0; 373 } 374 375 of = MFIFOGetFBOffset(buf); 376 ol = MFIFOGetLBOffset(buf); 377 378 if (mf->last == mf->first) { /* empty queue */ 379 mf->first = mf->last = 0; /* optimize */ 380 381 MFIFOWriteIX(buf, mes, size, mf->first, 0); 382 return 1; 383 } 384 else if (of > ol) { 385 if (mf->bsize - of >= size) { 386 MFIFOWriteIX(buf, mes, size, mf->first, of); 387 return 1; 388 } 389 else if (ol > size) { 390 MFIFOWriteIX(buf, mes, size, mf->first, ol); 391 return 1; 392 } 393 } 394 else if (ol - of > size) { 395 MFIFOWriteIX(buf, mes, size, mf->first, of); 396 return 1; 397 } 398 return 0; 399 }