Simple data buffer splitter and re-assembler.
DataFragmenterAssembler.cpp@0:12a931a6161c, 2015-02-12 (annotated)
- Committer:
- ansond
- Date:
- Thu Feb 12 20:00:36 2015 +0000
- Revision:
- 0:12a931a6161c
initial checkin
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ansond | 0:12a931a6161c | 1 | /** |
ansond | 0:12a931a6161c | 2 | * @file DataFragmenterAssembler.cpp |
ansond | 0:12a931a6161c | 3 | * @brief data fragmenter and assembler (for BLE UARTService...) implementation |
ansond | 0:12a931a6161c | 4 | * @author Doug Anson |
ansond | 0:12a931a6161c | 5 | * @version 1.0 |
ansond | 0:12a931a6161c | 6 | * @see |
ansond | 0:12a931a6161c | 7 | * |
ansond | 0:12a931a6161c | 8 | * Copyright (c) 2014 |
ansond | 0:12a931a6161c | 9 | * |
ansond | 0:12a931a6161c | 10 | * Licensed under the Apache License, Version 2.0 (the "License"); |
ansond | 0:12a931a6161c | 11 | * you may not use this file except in compliance with the License. |
ansond | 0:12a931a6161c | 12 | * You may obtain a copy of the License at |
ansond | 0:12a931a6161c | 13 | * |
ansond | 0:12a931a6161c | 14 | * http://www.apache.org/licenses/LICENSE-2.0 |
ansond | 0:12a931a6161c | 15 | * |
ansond | 0:12a931a6161c | 16 | * Unless required by applicable law or agreed to in writing, software |
ansond | 0:12a931a6161c | 17 | * distributed under the License is distributed on an "AS IS" BASIS, |
ansond | 0:12a931a6161c | 18 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
ansond | 0:12a931a6161c | 19 | * See the License for the specific language governing permissions and |
ansond | 0:12a931a6161c | 20 | * limitations under the License. |
ansond | 0:12a931a6161c | 21 | */ |
ansond | 0:12a931a6161c | 22 | |
ansond | 0:12a931a6161c | 23 | #include "DataFragmenterAssembler.h" |
ansond | 0:12a931a6161c | 24 | |
ansond | 0:12a931a6161c | 25 | // we have to redefine DBG as its used differently here... |
ansond | 0:12a931a6161c | 26 | #ifdef DBG |
ansond | 0:12a931a6161c | 27 | #undef DBG |
ansond | 0:12a931a6161c | 28 | #endif |
ansond | 0:12a931a6161c | 29 | #define DBG printf |
ansond | 0:12a931a6161c | 30 | |
ansond | 0:12a931a6161c | 31 | // constructor |
ansond | 0:12a931a6161c | 32 | DataFragmenterAssembler::DataFragmenterAssembler() |
ansond | 0:12a931a6161c | 33 | { |
ansond | 0:12a931a6161c | 34 | this->reset(); |
ansond | 0:12a931a6161c | 35 | } |
ansond | 0:12a931a6161c | 36 | |
ansond | 0:12a931a6161c | 37 | // fragment a buffer |
ansond | 0:12a931a6161c | 38 | int DataFragmenterAssembler::fragment(uint8_t *data,int data_length) |
ansond | 0:12a931a6161c | 39 | { |
ansond | 0:12a931a6161c | 40 | // reset |
ansond | 0:12a931a6161c | 41 | this->reset(); |
ansond | 0:12a931a6161c | 42 | |
ansond | 0:12a931a6161c | 43 | // get our number of fragments |
ansond | 0:12a931a6161c | 44 | this->m_num_fragments = this->calculateNumFragments(data,data_length); |
ansond | 0:12a931a6161c | 45 | |
ansond | 0:12a931a6161c | 46 | // DEBUG |
ansond | 0:12a931a6161c | 47 | //DBG("fragment() num_fragments=%d data_length=%d\r\n",this->m_num_fragments,data_length); |
ansond | 0:12a931a6161c | 48 | |
ansond | 0:12a931a6161c | 49 | // make sure we have a positive number... |
ansond | 0:12a931a6161c | 50 | if (this->m_num_fragments > 0) { |
ansond | 0:12a931a6161c | 51 | // check for the simple case first |
ansond | 0:12a931a6161c | 52 | if (this->m_num_fragments == 1) { |
ansond | 0:12a931a6161c | 53 | // simple case... just 1 fragment |
ansond | 0:12a931a6161c | 54 | memcpy(this->m_fragments[0],data,data_length); |
ansond | 0:12a931a6161c | 55 | this->m_last_fragment_length = data_length; |
ansond | 0:12a931a6161c | 56 | } |
ansond | 0:12a931a6161c | 57 | else { |
ansond | 0:12a931a6161c | 58 | // must iterate over the buffer and fragment... |
ansond | 0:12a931a6161c | 59 | for(int i=0;i<this->m_num_fragments;++i) { |
ansond | 0:12a931a6161c | 60 | int offset = i*DEF_FRAGMENT_LENGTH; |
ansond | 0:12a931a6161c | 61 | if (i < (this->m_num_fragments-1)) { |
ansond | 0:12a931a6161c | 62 | // interior... will always be fixed length |
ansond | 0:12a931a6161c | 63 | memcpy(this->m_fragments[i],(data + offset),DEF_FRAGMENT_LENGTH); |
ansond | 0:12a931a6161c | 64 | } |
ansond | 0:12a931a6161c | 65 | else { |
ansond | 0:12a931a6161c | 66 | // trailing... may be partial length... |
ansond | 0:12a931a6161c | 67 | this->m_last_fragment_length = data_length - offset; |
ansond | 0:12a931a6161c | 68 | memcpy(this->m_fragments[i],(data + offset),this->m_last_fragment_length); |
ansond | 0:12a931a6161c | 69 | } |
ansond | 0:12a931a6161c | 70 | } |
ansond | 0:12a931a6161c | 71 | } |
ansond | 0:12a931a6161c | 72 | } |
ansond | 0:12a931a6161c | 73 | else { |
ansond | 0:12a931a6161c | 74 | // unable to fragment... invalid parameters |
ansond | 0:12a931a6161c | 75 | DBG("ERROR: invalid parameters in fragment()\r\n"); |
ansond | 0:12a931a6161c | 76 | } |
ansond | 0:12a931a6161c | 77 | |
ansond | 0:12a931a6161c | 78 | // DEBUG |
ansond | 0:12a931a6161c | 79 | //this->dump(); |
ansond | 0:12a931a6161c | 80 | |
ansond | 0:12a931a6161c | 81 | // return our number of fragments |
ansond | 0:12a931a6161c | 82 | return this->m_num_fragments; |
ansond | 0:12a931a6161c | 83 | } |
ansond | 0:12a931a6161c | 84 | |
ansond | 0:12a931a6161c | 85 | // calculate the number of fragments |
ansond | 0:12a931a6161c | 86 | int DataFragmenterAssembler::calculateNumFragments(uint8_t *data,int data_length) |
ansond | 0:12a931a6161c | 87 | { |
ansond | 0:12a931a6161c | 88 | int num_fragments = 0; |
ansond | 0:12a931a6161c | 89 | |
ansond | 0:12a931a6161c | 90 | // param checking |
ansond | 0:12a931a6161c | 91 | if (data != NULL && data_length > 0) { |
ansond | 0:12a931a6161c | 92 | // check for simple case... |
ansond | 0:12a931a6161c | 93 | if (data_length <= DEF_FRAGMENT_LENGTH) { |
ansond | 0:12a931a6161c | 94 | num_fragments = 1; |
ansond | 0:12a931a6161c | 95 | } |
ansond | 0:12a931a6161c | 96 | else { |
ansond | 0:12a931a6161c | 97 | num_fragments = 1; |
ansond | 0:12a931a6161c | 98 | data_length -= DEF_FRAGMENT_LENGTH; |
ansond | 0:12a931a6161c | 99 | while(data_length > 0) { |
ansond | 0:12a931a6161c | 100 | ++num_fragments; |
ansond | 0:12a931a6161c | 101 | data_length -= DEF_FRAGMENT_LENGTH; |
ansond | 0:12a931a6161c | 102 | } |
ansond | 0:12a931a6161c | 103 | } |
ansond | 0:12a931a6161c | 104 | } |
ansond | 0:12a931a6161c | 105 | else { |
ansond | 0:12a931a6161c | 106 | // invalid parameters |
ansond | 0:12a931a6161c | 107 | DBG("ERROR: invalid parameters in calculateNumFragments() data_length=%d\r\n",data_length); |
ansond | 0:12a931a6161c | 108 | } |
ansond | 0:12a931a6161c | 109 | |
ansond | 0:12a931a6161c | 110 | return num_fragments; |
ansond | 0:12a931a6161c | 111 | } |
ansond | 0:12a931a6161c | 112 | |
ansond | 0:12a931a6161c | 113 | // get the ith fragment |
ansond | 0:12a931a6161c | 114 | uint8_t *DataFragmenterAssembler::get(int index) |
ansond | 0:12a931a6161c | 115 | { |
ansond | 0:12a931a6161c | 116 | if (index >= 0 && index < this->m_num_fragments) |
ansond | 0:12a931a6161c | 117 | return this->m_fragments[index]; |
ansond | 0:12a931a6161c | 118 | return NULL; |
ansond | 0:12a931a6161c | 119 | } |
ansond | 0:12a931a6161c | 120 | |
ansond | 0:12a931a6161c | 121 | // reset the Fragmenter/Assembler |
ansond | 0:12a931a6161c | 122 | void DataFragmenterAssembler::reset(void) |
ansond | 0:12a931a6161c | 123 | { |
ansond | 0:12a931a6161c | 124 | memset(this->m_fragments,0,sizeof(this->m_fragments)); |
ansond | 0:12a931a6161c | 125 | this->m_num_fragments = 0; |
ansond | 0:12a931a6161c | 126 | this->m_last_fragment_length = 0; |
ansond | 0:12a931a6161c | 127 | } |
ansond | 0:12a931a6161c | 128 | |
ansond | 0:12a931a6161c | 129 | // add a fragment to assemble later |
ansond | 0:12a931a6161c | 130 | int DataFragmenterAssembler::add(uint8_t *fragment,int fragment_length) |
ansond | 0:12a931a6161c | 131 | { |
ansond | 0:12a931a6161c | 132 | if (this->m_num_fragments < (MAX_FRAGMENTS-1)) { |
ansond | 0:12a931a6161c | 133 | int length = fragment_length; |
ansond | 0:12a931a6161c | 134 | if (length > DEF_FRAGMENT_LENGTH) { |
ansond | 0:12a931a6161c | 135 | length = DEF_FRAGMENT_LENGTH; |
ansond | 0:12a931a6161c | 136 | //DBG("WARNING: Truncating input fragment in add() fragment_length=%d\r\n",fragment_length); |
ansond | 0:12a931a6161c | 137 | } |
ansond | 0:12a931a6161c | 138 | memcpy(this->m_fragments[this->m_num_fragments],fragment,length); |
ansond | 0:12a931a6161c | 139 | ++this->m_num_fragments; |
ansond | 0:12a931a6161c | 140 | } |
ansond | 0:12a931a6161c | 141 | else { |
ansond | 0:12a931a6161c | 142 | // not enough memory to hold all the fragments |
ansond | 0:12a931a6161c | 143 | DBG("ERROR: Maximum number of fragments permissible reached, Please increase MAX_FRAGMENTS...\r\n"); |
ansond | 0:12a931a6161c | 144 | } |
ansond | 0:12a931a6161c | 145 | return this->m_num_fragments; |
ansond | 0:12a931a6161c | 146 | } |
ansond | 0:12a931a6161c | 147 | |
ansond | 0:12a931a6161c | 148 | // assemble all input fragments |
ansond | 0:12a931a6161c | 149 | void DataFragmenterAssembler::assemble(uint8_t *buffer,int buffer_length,bool reset_after_assemble) |
ansond | 0:12a931a6161c | 150 | { |
ansond | 0:12a931a6161c | 151 | // calculate the final assembly length |
ansond | 0:12a931a6161c | 152 | int length = this->calculateAssemblyLength(buffer_length); |
ansond | 0:12a931a6161c | 153 | if (length > 0) { |
ansond | 0:12a931a6161c | 154 | // initialize the return buffer |
ansond | 0:12a931a6161c | 155 | memset(buffer,0,buffer_length); |
ansond | 0:12a931a6161c | 156 | |
ansond | 0:12a931a6161c | 157 | // check for the simple case.. 1 fragment |
ansond | 0:12a931a6161c | 158 | if (this->m_num_fragments == 1) { |
ansond | 0:12a931a6161c | 159 | // simple case detected... just copy over |
ansond | 0:12a931a6161c | 160 | memset(buffer,0,buffer_length); |
ansond | 0:12a931a6161c | 161 | memcpy(buffer,this->m_fragments[0],length); |
ansond | 0:12a931a6161c | 162 | } |
ansond | 0:12a931a6161c | 163 | else { |
ansond | 0:12a931a6161c | 164 | // we have to loop and copy/append |
ansond | 0:12a931a6161c | 165 | for(int i=0;i<this->m_num_fragments;++i) { |
ansond | 0:12a931a6161c | 166 | int offset = (i*DEF_FRAGMENT_LENGTH); |
ansond | 0:12a931a6161c | 167 | if (i < (this->m_num_fragments-1)) { |
ansond | 0:12a931a6161c | 168 | // interior... will always be fixed length |
ansond | 0:12a931a6161c | 169 | memcpy((buffer+offset),this->m_fragments[i],DEF_FRAGMENT_LENGTH); |
ansond | 0:12a931a6161c | 170 | } |
ansond | 0:12a931a6161c | 171 | else { |
ansond | 0:12a931a6161c | 172 | // trailing... may be partial length... |
ansond | 0:12a931a6161c | 173 | memcpy((buffer+offset),this->m_fragments[i],this->m_last_fragment_length); |
ansond | 0:12a931a6161c | 174 | } |
ansond | 0:12a931a6161c | 175 | } |
ansond | 0:12a931a6161c | 176 | } |
ansond | 0:12a931a6161c | 177 | |
ansond | 0:12a931a6161c | 178 | // DEBUG |
ansond | 0:12a931a6161c | 179 | //DBG("assemble(): buffer=[%s] length=%d\r\n",buffer,buffer_length); |
ansond | 0:12a931a6161c | 180 | } |
ansond | 0:12a931a6161c | 181 | else { |
ansond | 0:12a931a6161c | 182 | // unable to assemble... |
ansond | 0:12a931a6161c | 183 | DBG("ERROR: Unable to assemble. calculateAssemblyLength() failed (%d)\r\n",length); |
ansond | 0:12a931a6161c | 184 | } |
ansond | 0:12a931a6161c | 185 | } |
ansond | 0:12a931a6161c | 186 | |
ansond | 0:12a931a6161c | 187 | // calculate the assembled packet length |
ansond | 0:12a931a6161c | 188 | int DataFragmenterAssembler::calculateAssemblyLength(int buffer_length) { |
ansond | 0:12a931a6161c | 189 | int length = 0; |
ansond | 0:12a931a6161c | 190 | |
ansond | 0:12a931a6161c | 191 | for(int i=0;i<this->m_num_fragments;++i) { |
ansond | 0:12a931a6161c | 192 | if (i < (this->m_num_fragments-1)) length += DEF_FRAGMENT_LENGTH; |
ansond | 0:12a931a6161c | 193 | else length += this->m_last_fragment_length; |
ansond | 0:12a931a6161c | 194 | } |
ansond | 0:12a931a6161c | 195 | |
ansond | 0:12a931a6161c | 196 | // sanity check |
ansond | 0:12a931a6161c | 197 | if (length > buffer_length) { |
ansond | 0:12a931a6161c | 198 | // input buffer is too small... |
ansond | 0:12a931a6161c | 199 | DBG("ERROR: calculateAssemblyLength() input buffer too small: %d bytes. required length: %d bytes.\r\n",buffer_length,length); |
ansond | 0:12a931a6161c | 200 | length = -1; |
ansond | 0:12a931a6161c | 201 | } |
ansond | 0:12a931a6161c | 202 | |
ansond | 0:12a931a6161c | 203 | return length; |
ansond | 0:12a931a6161c | 204 | } |
ansond | 0:12a931a6161c | 205 | |
ansond | 0:12a931a6161c | 206 | // dump the state of the Fragmenter/Assembler |
ansond | 0:12a931a6161c | 207 | void DataFragmenterAssembler::dump(void) |
ansond | 0:12a931a6161c | 208 | { |
ansond | 0:12a931a6161c | 209 | DBG("\r\nDUMP: Number of fragments: %d last_length=%d\r\n",this->m_num_fragments,this->m_last_fragment_length); |
ansond | 0:12a931a6161c | 210 | for(int i=0;i<this->m_num_fragments;++i) { |
ansond | 0:12a931a6161c | 211 | DBG("DUMP: Fragment[%d]=[%s] length=%d\r\n",i,this->m_fragments[i],strlen((const char *)this->m_fragments[i])); |
ansond | 0:12a931a6161c | 212 | } |
ansond | 0:12a931a6161c | 213 | DBG("\r\n"); |
ansond | 0:12a931a6161c | 214 | } |
ansond | 0:12a931a6161c | 215 | |
ansond | 0:12a931a6161c | 216 | |
ansond | 0:12a931a6161c | 217 |