Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 1 | /**************************************************************************** |
| 2 | |
| 3 | (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29 |
| 4 | www.systec-electronic.com |
| 5 | |
| 6 | Project: openPOWERLINK |
| 7 | |
| 8 | Description: source file for user PDO module |
| 9 | Currently, this module just implements a OD callback function |
| 10 | to check if the PDO configuration is valid. |
| 11 | |
| 12 | License: |
| 13 | |
| 14 | Redistribution and use in source and binary forms, with or without |
| 15 | modification, are permitted provided that the following conditions |
| 16 | are met: |
| 17 | |
| 18 | 1. Redistributions of source code must retain the above copyright |
| 19 | notice, this list of conditions and the following disclaimer. |
| 20 | |
| 21 | 2. Redistributions in binary form must reproduce the above copyright |
| 22 | notice, this list of conditions and the following disclaimer in the |
| 23 | documentation and/or other materials provided with the distribution. |
| 24 | |
| 25 | 3. Neither the name of SYSTEC electronic GmbH nor the names of its |
| 26 | contributors may be used to endorse or promote products derived |
| 27 | from this software without prior written permission. For written |
| 28 | permission, please contact info@systec-electronic.com. |
| 29 | |
| 30 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 31 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 32 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 33 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 34 | COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 35 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 36 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 37 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 38 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 39 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 40 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 41 | POSSIBILITY OF SUCH DAMAGE. |
| 42 | |
| 43 | Severability Clause: |
| 44 | |
| 45 | If a provision of this License is or becomes illegal, invalid or |
| 46 | unenforceable in any jurisdiction, that shall not affect: |
| 47 | 1. the validity or enforceability in that jurisdiction of any other |
| 48 | provision of this License; or |
| 49 | 2. the validity or enforceability in other jurisdictions of that or |
| 50 | any other provision of this License. |
| 51 | |
| 52 | ------------------------------------------------------------------------- |
| 53 | |
| 54 | $RCSfile: EplPdou.c,v $ |
| 55 | |
| 56 | $Author: D.Krueger $ |
| 57 | |
| 58 | $Revision: 1.5 $ $Date: 2008/10/17 15:32:32 $ |
| 59 | |
| 60 | $State: Exp $ |
| 61 | |
| 62 | Build Environment: |
| 63 | GCC V3.4 |
| 64 | |
| 65 | ------------------------------------------------------------------------- |
| 66 | |
| 67 | Revision History: |
| 68 | |
| 69 | 2006/05/22 d.k.: start of the implementation, version 1.00 |
| 70 | |
| 71 | ****************************************************************************/ |
| 72 | |
| 73 | #include "EplInc.h" |
| 74 | //#include "user/EplPdouCal.h" |
| 75 | #include "user/EplObdu.h" |
| 76 | #include "user/EplPdou.h" |
| 77 | #include "EplSdoAc.h" |
| 78 | |
| 79 | #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0) |
| 80 | |
| 81 | #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE) |
| 82 | #error "EPL PDOu module needs EPL module OBDU or OBDK!" |
| 83 | #endif |
| 84 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 85 | /***************************************************************************/ |
| 86 | /* */ |
| 87 | /* */ |
| 88 | /* G L O B A L D E F I N I T I O N S */ |
| 89 | /* */ |
| 90 | /* */ |
| 91 | /***************************************************************************/ |
| 92 | |
| 93 | //--------------------------------------------------------------------------- |
| 94 | // const defines |
| 95 | //--------------------------------------------------------------------------- |
| 96 | |
| 97 | #define EPL_PDOU_OBD_IDX_RX_COMM_PARAM 0x1400 |
| 98 | #define EPL_PDOU_OBD_IDX_RX_MAPP_PARAM 0x1600 |
| 99 | #define EPL_PDOU_OBD_IDX_TX_COMM_PARAM 0x1800 |
| 100 | #define EPL_PDOU_OBD_IDX_TX_MAPP_PARAM 0x1A00 |
| 101 | #define EPL_PDOU_OBD_IDX_MAPP_PARAM 0x0200 |
| 102 | #define EPL_PDOU_OBD_IDX_MASK 0xFF00 |
| 103 | #define EPL_PDOU_PDO_ID_MASK 0x00FF |
| 104 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 105 | //--------------------------------------------------------------------------- |
| 106 | // local types |
| 107 | //--------------------------------------------------------------------------- |
| 108 | |
| 109 | //--------------------------------------------------------------------------- |
| 110 | // modul globale vars |
| 111 | //--------------------------------------------------------------------------- |
| 112 | |
| 113 | //--------------------------------------------------------------------------- |
| 114 | // local function prototypes |
| 115 | //--------------------------------------------------------------------------- |
| 116 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 117 | /***************************************************************************/ |
| 118 | /* */ |
| 119 | /* */ |
| 120 | /* C L A S S EplPdou */ |
| 121 | /* */ |
| 122 | /* */ |
| 123 | /***************************************************************************/ |
| 124 | // |
| 125 | // Description: |
| 126 | // |
| 127 | // |
| 128 | /***************************************************************************/ |
| 129 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 130 | //=========================================================================// |
| 131 | // // |
| 132 | // P R I V A T E D E F I N I T I O N S // |
| 133 | // // |
| 134 | //=========================================================================// |
| 135 | |
| 136 | //--------------------------------------------------------------------------- |
| 137 | // const defines |
| 138 | //--------------------------------------------------------------------------- |
| 139 | |
| 140 | //--------------------------------------------------------------------------- |
| 141 | // local types |
| 142 | //--------------------------------------------------------------------------- |
| 143 | |
| 144 | //--------------------------------------------------------------------------- |
| 145 | // local vars |
| 146 | //--------------------------------------------------------------------------- |
| 147 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 148 | //--------------------------------------------------------------------------- |
| 149 | // local function prototypes |
| 150 | //--------------------------------------------------------------------------- |
| 151 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 152 | static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p, |
| 153 | unsigned int uiIndex_p); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 154 | |
| 155 | static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p, |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 156 | unsigned int *puiIndex_p, |
| 157 | unsigned int *puiSubIndex_p, |
| 158 | unsigned int *puiBitOffset_p, |
| 159 | unsigned int *puiBitSize_p); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 160 | |
| 161 | static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p, |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 162 | tEplObdAccess AccessType_p, |
| 163 | DWORD * pdwAbortCode_p, |
| 164 | unsigned int *puiPdoSize_p); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 165 | |
| 166 | //=========================================================================// |
| 167 | // // |
| 168 | // P U B L I C F U N C T I O N S // |
| 169 | // // |
| 170 | //=========================================================================// |
| 171 | |
| 172 | //--------------------------------------------------------------------------- |
| 173 | // |
| 174 | // Function: EplPdouAddInstance() |
| 175 | // |
| 176 | // Description: add and initialize new instance of EPL stack |
| 177 | // |
| 178 | // Parameters: none |
| 179 | // |
| 180 | // Returns: tEplKernel = error code |
| 181 | // |
| 182 | // |
| 183 | // State: |
| 184 | // |
| 185 | //--------------------------------------------------------------------------- |
| 186 | |
| 187 | tEplKernel EplPdouAddInstance(void) |
| 188 | { |
| 189 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 190 | return kEplSuccessful; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 191 | } |
| 192 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 193 | //--------------------------------------------------------------------------- |
| 194 | // |
| 195 | // Function: EplPdouDelInstance() |
| 196 | // |
| 197 | // Description: deletes an instance of EPL stack |
| 198 | // |
| 199 | // Parameters: none |
| 200 | // |
| 201 | // Returns: tEplKernel = error code |
| 202 | // |
| 203 | // |
| 204 | // State: |
| 205 | // |
| 206 | //--------------------------------------------------------------------------- |
| 207 | |
| 208 | tEplKernel EplPdouDelInstance(void) |
| 209 | { |
| 210 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 211 | return kEplSuccessful; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 212 | } |
| 213 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 214 | //--------------------------------------------------------------------------- |
| 215 | // |
| 216 | // Function: EplPdouCbObdAccess |
| 217 | // |
| 218 | // Description: callback function for OD accesses |
| 219 | // |
| 220 | // Parameters: pParam_p = OBD parameter |
| 221 | // |
| 222 | // Returns: tEplKernel = error code |
| 223 | // |
| 224 | // |
| 225 | // State: |
| 226 | // |
| 227 | //--------------------------------------------------------------------------- |
| 228 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 229 | tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM * pParam_p) |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 230 | { |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 231 | tEplKernel Ret = kEplSuccessful; |
| 232 | unsigned int uiPdoId; |
| 233 | unsigned int uiIndexType; |
| 234 | tEplObdSize ObdSize; |
| 235 | BYTE bObjectCount; |
| 236 | QWORD qwObjectMapping; |
| 237 | tEplObdAccess AccessType; |
| 238 | BYTE bMappSubindex; |
| 239 | unsigned int uiCurPdoSize; |
| 240 | WORD wMaxPdoSize; |
| 241 | unsigned int uiSubIndex; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 242 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 243 | // fetch PDO ID |
| 244 | uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 245 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 246 | // fetch object index type |
| 247 | uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 248 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 249 | if (pParam_p->m_ObdEvent != kEplObdEvPreWrite) { // read accesses, post write events etc. are OK |
| 250 | pParam_p->m_dwAbortCode = 0; |
| 251 | goto Exit; |
| 252 | } |
| 253 | // check index type |
| 254 | switch (uiIndexType) { |
| 255 | case EPL_PDOU_OBD_IDX_RX_COMM_PARAM: |
| 256 | // RPDO communication parameter accessed |
| 257 | case EPL_PDOU_OBD_IDX_TX_COMM_PARAM: |
| 258 | { // TPDO communication parameter accessed |
| 259 | Ret = EplPdouCheckPdoValidity(pParam_p, |
| 260 | (EPL_PDOU_OBD_IDX_MAPP_PARAM |
| 261 | | pParam_p->m_uiIndex)); |
| 262 | if (Ret != kEplSuccessful) { // PDO is valid or does not exist |
| 263 | goto Exit; |
| 264 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 265 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 266 | goto Exit; |
| 267 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 268 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 269 | case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM: |
| 270 | { // RPDO mapping parameter accessed |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 271 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 272 | AccessType = kEplObdAccWrite; |
| 273 | break; |
| 274 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 275 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 276 | case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM: |
| 277 | { // TPDO mapping parameter accessed |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 278 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 279 | AccessType = kEplObdAccRead; |
| 280 | break; |
| 281 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 282 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 283 | default: |
| 284 | { // this callback function is only for |
| 285 | // PDO mapping and communication parameters |
| 286 | pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; |
| 287 | goto Exit; |
| 288 | } |
| 289 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 290 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 291 | // RPDO and TPDO mapping parameter accessed |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 292 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 293 | if (pParam_p->m_uiSubIndex == 0) { // object mapping count accessed |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 294 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 295 | // PDO is enabled or disabled |
| 296 | bObjectCount = *((BYTE *) pParam_p->m_pArg); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 297 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 298 | if (bObjectCount == 0) { // PDO shall be disabled |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 299 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 300 | // that is always possible |
| 301 | goto Exit; |
| 302 | } |
| 303 | // PDO shall be enabled |
| 304 | // it should have been disabled for this operation |
| 305 | Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex); |
| 306 | if (Ret != kEplSuccessful) { // PDO is valid or does not exist |
| 307 | goto Exit; |
| 308 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 309 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 310 | if (AccessType == kEplObdAccWrite) { |
| 311 | uiSubIndex = 0x04; // PReqActPayloadLimit_U16 |
| 312 | } else { |
| 313 | uiSubIndex = 0x05; // PResActPayloadLimit_U16 |
| 314 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 315 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 316 | // fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC |
| 317 | ObdSize = sizeof(wMaxPdoSize); |
| 318 | Ret = |
| 319 | EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize, |
| 320 | &ObdSize); |
| 321 | if (Ret != kEplSuccessful) { // other fatal error occured |
| 322 | pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR; |
| 323 | goto Exit; |
| 324 | } |
| 325 | // check all objectmappings |
| 326 | for (bMappSubindex = 1; bMappSubindex <= bObjectCount; |
| 327 | bMappSubindex++) { |
| 328 | // read object mapping from OD |
| 329 | ObdSize = sizeof(qwObjectMapping); // QWORD |
| 330 | Ret = EplObduReadEntry(pParam_p->m_uiIndex, |
| 331 | bMappSubindex, &qwObjectMapping, |
| 332 | &ObdSize); |
| 333 | if (Ret != kEplSuccessful) { // other fatal error occured |
| 334 | pParam_p->m_dwAbortCode = |
| 335 | EPL_SDOAC_GENERAL_ERROR; |
| 336 | goto Exit; |
| 337 | } |
| 338 | // check object mapping |
| 339 | Ret = EplPdouCheckObjectMapping(qwObjectMapping, |
| 340 | AccessType, |
| 341 | &pParam_p-> |
| 342 | m_dwAbortCode, |
| 343 | &uiCurPdoSize); |
| 344 | if (Ret != kEplSuccessful) { // illegal object mapping |
| 345 | goto Exit; |
| 346 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 347 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 348 | if (uiCurPdoSize > wMaxPdoSize) { // mapping exceeds object size |
| 349 | pParam_p->m_dwAbortCode = |
| 350 | EPL_SDOAC_GENERAL_ERROR; |
| 351 | Ret = kEplPdoVarNotFound; |
| 352 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 353 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 354 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 355 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 356 | } else { // ObjectMapping |
| 357 | Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex); |
| 358 | if (Ret != kEplSuccessful) { // PDO is valid or does not exist |
| 359 | goto Exit; |
| 360 | } |
| 361 | // check existence of object and validity of object length |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 362 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 363 | qwObjectMapping = *((QWORD *) pParam_p->m_pArg); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 364 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 365 | Ret = EplPdouCheckObjectMapping(qwObjectMapping, |
| 366 | AccessType, |
| 367 | &pParam_p->m_dwAbortCode, |
| 368 | &uiCurPdoSize); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 369 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 370 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 371 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 372 | Exit: |
| 373 | return Ret; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 374 | } |
| 375 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 376 | //=========================================================================// |
| 377 | // // |
| 378 | // P R I V A T E F U N C T I O N S // |
| 379 | // // |
| 380 | //=========================================================================// |
| 381 | |
| 382 | //--------------------------------------------------------------------------- |
| 383 | // |
| 384 | // Function: EplPdouCheckPdoValidity |
| 385 | // |
| 386 | // Description: check if PDO is valid |
| 387 | // |
| 388 | // Parameters: pParam_p = OBD parameter |
| 389 | // |
| 390 | // Returns: tEplKernel = error code |
| 391 | // |
| 392 | // |
| 393 | // State: |
| 394 | // |
| 395 | //--------------------------------------------------------------------------- |
| 396 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 397 | static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p, |
| 398 | unsigned int uiIndex_p) |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 399 | { |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 400 | tEplKernel Ret = kEplSuccessful; |
| 401 | tEplObdSize ObdSize; |
| 402 | BYTE bObjectCount; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 403 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 404 | ObdSize = 1; |
| 405 | // read number of mapped objects from OD; this indicates if the PDO is valid |
| 406 | Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize); |
| 407 | if (Ret != kEplSuccessful) { // other fatal error occured |
| 408 | pParam_p->m_dwAbortCode = |
| 409 | EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY; |
| 410 | goto Exit; |
| 411 | } |
| 412 | // entry read successfully |
| 413 | if (bObjectCount != 0) { // PDO in OD is still valid |
| 414 | pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY; |
| 415 | Ret = kEplPdoNotExist; |
| 416 | goto Exit; |
| 417 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 418 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 419 | Exit: |
| 420 | return Ret; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 421 | } |
| 422 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 423 | //--------------------------------------------------------------------------- |
| 424 | // |
| 425 | // Function: EplPdouDecodeObjectMapping |
| 426 | // |
| 427 | // Description: decodes the given object mapping entry into index, subindex, |
| 428 | // bit offset and bit size. |
| 429 | // |
| 430 | // Parameters: qwObjectMapping_p = object mapping entry |
| 431 | // puiIndex_p = [OUT] pointer to object index |
| 432 | // puiSubIndex_p = [OUT] pointer to subindex |
| 433 | // puiBitOffset_p = [OUT] pointer to bit offset |
| 434 | // puiBitSize_p = [OUT] pointer to bit size |
| 435 | // |
| 436 | // Returns: (void) |
| 437 | // |
| 438 | // State: |
| 439 | // |
| 440 | //--------------------------------------------------------------------------- |
| 441 | |
| 442 | static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p, |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 443 | unsigned int *puiIndex_p, |
| 444 | unsigned int *puiSubIndex_p, |
| 445 | unsigned int *puiBitOffset_p, |
| 446 | unsigned int *puiBitSize_p) |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 447 | { |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 448 | *puiIndex_p = (unsigned int) |
| 449 | (qwObjectMapping_p & 0x000000000000FFFFLL); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 450 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 451 | *puiSubIndex_p = (unsigned int) |
| 452 | ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 453 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 454 | *puiBitOffset_p = (unsigned int) |
| 455 | ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 456 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 457 | *puiBitSize_p = (unsigned int) |
| 458 | ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 459 | |
| 460 | } |
| 461 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 462 | //--------------------------------------------------------------------------- |
| 463 | // |
| 464 | // Function: EplPdouCheckObjectMapping |
| 465 | // |
| 466 | // Description: checks the given object mapping entry. |
| 467 | // |
| 468 | // Parameters: qwObjectMapping_p = object mapping entry |
| 469 | // AccessType_p = access type to mapped object: |
| 470 | // write = RPDO and read = TPDO |
| 471 | // puiPdoSize_p = [OUT] pointer to covered PDO size |
| 472 | // (offset + size) in byte; |
| 473 | // 0 if mapping failed |
| 474 | // pdwAbortCode_p = [OUT] pointer to SDO abort code; |
| 475 | // 0 if mapping is possible |
| 476 | // |
| 477 | // Returns: tEplKernel = error code |
| 478 | // |
| 479 | // State: |
| 480 | // |
| 481 | //--------------------------------------------------------------------------- |
| 482 | |
| 483 | static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p, |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 484 | tEplObdAccess AccessType_p, |
| 485 | DWORD * pdwAbortCode_p, |
| 486 | unsigned int *puiPdoSize_p) |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 487 | { |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 488 | tEplKernel Ret = kEplSuccessful; |
| 489 | tEplObdSize ObdSize; |
| 490 | unsigned int uiIndex; |
| 491 | unsigned int uiSubIndex; |
| 492 | unsigned int uiBitOffset; |
| 493 | unsigned int uiBitSize; |
| 494 | tEplObdAccess AccessType; |
| 495 | BOOL fNumerical; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 496 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 497 | if (qwObjectMapping_p == 0) { // discard zero value |
| 498 | *puiPdoSize_p = 0; |
| 499 | goto Exit; |
| 500 | } |
| 501 | // decode object mapping |
| 502 | EplPdouDecodeObjectMapping(qwObjectMapping_p, |
| 503 | &uiIndex, |
| 504 | &uiSubIndex, &uiBitOffset, &uiBitSize); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 505 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 506 | if ((uiBitOffset & 0x7) != 0x0) { // bit mapping is not supported |
| 507 | *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; |
| 508 | Ret = kEplPdoGranularityMismatch; |
| 509 | goto Exit; |
| 510 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 511 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 512 | if ((uiBitSize & 0x7) != 0x0) { // bit mapping is not supported |
| 513 | *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; |
| 514 | Ret = kEplPdoGranularityMismatch; |
| 515 | goto Exit; |
| 516 | } |
| 517 | // check access type |
| 518 | Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType); |
| 519 | if (Ret != kEplSuccessful) { // entry doesn't exist |
| 520 | *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST; |
| 521 | goto Exit; |
| 522 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 523 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 524 | if ((AccessType & kEplObdAccPdo) == 0) { // object is not mappable |
| 525 | *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE; |
| 526 | Ret = kEplPdoVarNotFound; |
| 527 | goto Exit; |
| 528 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 529 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 530 | if ((AccessType & AccessType_p) == 0) { // object is not writeable (RPDO) or readable (TPDO) respectively |
| 531 | *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE; |
| 532 | Ret = kEplPdoVarNotFound; |
| 533 | goto Exit; |
| 534 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 535 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 536 | ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex); |
| 537 | if (ObdSize < (uiBitSize >> 3)) { // object does not exist or has smaller size |
| 538 | *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; |
| 539 | Ret = kEplPdoVarNotFound; |
| 540 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 541 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 542 | Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical); |
| 543 | if (Ret != kEplSuccessful) { // entry doesn't exist |
| 544 | *pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST; |
| 545 | goto Exit; |
| 546 | } |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 547 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 548 | if ((fNumerical != FALSE) |
| 549 | && ((uiBitSize >> 3) != ObdSize)) { |
| 550 | // object is numerical, |
| 551 | // therefor size has to fit, but it does not. |
| 552 | *pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR; |
| 553 | Ret = kEplPdoVarNotFound; |
| 554 | goto Exit; |
| 555 | } |
| 556 | // calucaled needed PDO size |
| 557 | *puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3); |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 558 | |
Greg Kroah-Hartman | 833dfbe | 2008-12-19 17:11:52 -0800 | [diff] [blame] | 559 | Exit: |
| 560 | return Ret; |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 561 | } |
| 562 | |
Daniel Krueger | 9d7164c | 2008-12-19 11:41:57 -0800 | [diff] [blame] | 563 | #endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0) |
| 564 | |
| 565 | // EOF |