1  /*
  2      EIBD eib bus access and management daemon
  3      Copyright (C) 2005-2011 Martin Koegler <mkoegler@auto.tuwien.ac.at>
  4  
  5      This program is free software; you can redistribute it and/or modify
  6      it under the terms of the GNU General Public License as published by
  7      the Free Software Foundation; either version 2 of the License, or
  8      (at your option) any later version.
  9  
 10      This program is distributed in the hope that it will be useful,
 11      but WITHOUT ANY WARRANTY; without even the implied warranty of
 12      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13      GNU General Public License for more details.
 14  
 15      You should have received a copy of the GNU General Public License
 16      along with this program; if not, write to the Free Software
 17      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 18  */
 19  
 20  #include "cm_tp1.h"
 21  
 22  /* L_Data */
 23  
 24  LDataPtr CM_TP1_to_L_Data (const CArray & c, TracePtr)
 25  {
 26    LDataPtr l = LDataPtr(new L_Data_PDU ());
 27    if (c.size() < 6)
 28      return nullptr;
 29    if ((c[0] & 0x53) != 0x10)
 30      return nullptr;
 31    l->frame_format = (c[0] & 0x80) ? 1 : 0;
 32    l->repeated = (c[0] & 0x20) ? 0 : 1;
 33    l->priority = static_cast<EIB_Priority>((c[0] >> 2) & 0x3);
 34    l->valid_length = 1;
 35    if (l->frame_format)
 36      {
 37        /* Standard frame */
 38        l->source_address = (c[1] << 8) | (c[2]);
 39        l->destination_address = (c[3] << 8) | (c[4]);
 40        l->address_type = (c[5] & 0x80) ? GroupAddress : IndividualAddress;
 41        l->hop_count = (c[5] >> 4) & 0x07; // @todo this is NPDU
 42        uint8_t len = (c[5] & 0x0f) + 1;
 43        if (len + 7 != c.size())
 44          return nullptr;
 45        l->lsdu.set (c.data() + 6, len);
 46      }
 47    else
 48      {
 49        /* extended frame */
 50        if ((c[1] & 0x0f) != 0)
 51          return nullptr;
 52        if (c.size() < 7)
 53          return nullptr;
 54        l->address_type = (c[1] & 0x80) ? GroupAddress : IndividualAddress;
 55        l->hop_count = (c[1] >> 4) & 0x07; // @todo this is NPDU
 56        l->source_address = (c[2] << 8) | (c[3]);
 57        l->destination_address = (c[4] << 8) | (c[5]);
 58        uint8_t len = c[6] + 1;
 59        if (len + 8 != c.size())
 60          {
 61            if (c.size() == 23)
 62              {
 63                l->valid_length = 0;
 64                l->lsdu.set (c.data() + 7, 8);
 65              }
 66            else
 67              return nullptr;
 68          }
 69        else
 70          l->lsdu.set (c.data() + 7, len);
 71      }
 72  
 73    /* checksum */
 74    uint8_t checksum = 0;
 75    for (unsigned i = 0; i < c.size () - 1; i++)
 76      checksum ^= c[i];
 77    checksum = ~checksum;
 78    l->valid_checksum = (c[c.size () - 1] == checksum);
 79  
 80    return l;
 81  }
 82  
 83  CArray L_Data_to_CM_TP1 (const LDataPtr & p)
 84  {
 85    assert (p->lsdu.size() >= 1);
 86    assert (p->lsdu.size() <= 0xff);
 87    assert ((p->hop_count & 0xf8) == 0);
 88  
 89    CArray pdu;
 90    uint8_t len = p->lsdu.size() - 1;
 91    if (len <= 0x0f)
 92      {
 93        /* L_Data_Standard Frame */
 94        pdu.resize (7 + p->lsdu.size());
 95        pdu[0] = 0x90 | (p->repeated ? 0x00 : 0x20) | (p->priority << 2);
 96        pdu[1] = p->source_address >> 8;
 97        pdu[2] = p->source_address & 0xff;
 98        pdu[3] = p->destination_address >> 8;
 99        pdu[4] = p->destination_address & 0xff;
100        pdu[5] =
101          (p->address_type == GroupAddress ? 0x80 : 0x00) |
102          ((p->hop_count & 0x07) << 4) |
103          (len & 0x0f);
104        pdu.setpart (p->lsdu.data(), 6, 1 + (len & 0x0f));
105      }
106    else
107      {
108        /* L_Data_Extended Frame */
109        pdu.resize (8 + p->lsdu.size());
110        pdu[0] = 0x10 | (p->repeated ? 0x00 : 0x20) | (p->priority << 2);
111        pdu[1] =
112          (p->address_type == GroupAddress ? 0x80 : 0x00) |
113          ((p->hop_count & 0x07) << 4);
114        pdu[2] = p->source_address >> 8;
115        pdu[3] = p->source_address & 0xff;
116        pdu[4] = p->destination_address >> 8;
117        pdu[5] = p->destination_address & 0xff;
118        pdu[6] = (p->lsdu.size() - 1) & 0xff;
119        pdu.setpart (p->lsdu.data(), 7, 1 + (len & 0xff));
120      }
121  
122    /* checksum */
123    uint8_t checksum = 0;
124    for (unsigned i = 0; i < pdu.size() - 1; i++)
125      checksum ^= pdu[i];
126    pdu[pdu.size() - 1] = ~checksum;
127  
128    return pdu;
129  }