eibd/libserver/lpdu.cpp

   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 <stdio.h>
  21 #include "lpdu.h"
  22 #include "tpdu.h"
  23
  24 LPDU *
  25 LPDU::fromPacket (const CArray & c)
  26 {
  27   LPDU *l = 0;
  28   if (c () >= 1)
  29     {
  30       if (c[0] == 0xCC)
  31         l = new L_ACK_PDU ();
  32       if (c[0] == 0xC0)
  33         l = new L_BUSY_PDU ();
  34       if (c[0] == 0x0C)
  35         l = new L_NACK_PDU ();
  36       if ((c[0] & 0x53) == 0x10)
  37         l = new L_Data_PDU ();
  38     }
  39   if (l && l->init (c))
  40     return l;
  41   if (l)
  42     delete l;
  43   l = new L_Unknown_PDU ();
  44   l->init (c);
  45   return l;
  46 }
  47
  48 /* L_NACK */
  49
  50 L_NACK_PDU::L_NACK_PDU ()
  51 {
  52 }
  53
  54 bool
  55 L_NACK_PDU::init (const CArray & c)
  56 {
  57   if (c () != 1)
  58     return false;
  59   return true;
  60 }
  61
  62 CArray L_NACK_PDU::ToPacket ()
  63 {
  64   uchar
  65     c = 0x0C;
  66   return CArray (&c, 1);
  67 }
  68
  69 String L_NACK_PDU::Decode ()
  70 {
  71   return "NACK";
  72 }
  73
  74 /* L_ACK */
  75
  76 L_ACK_PDU::L_ACK_PDU ()
  77 {
  78 }
  79
  80 bool
  81 L_ACK_PDU::init (const CArray & c)
  82 {
  83   if (c () != 1)
  84     return false;
  85   return true;
  86 }
  87
  88 CArray L_ACK_PDU::ToPacket ()
  89 {
  90   uchar
  91     c = 0xCC;
  92   return CArray (&c, 1);
  93 }
  94
  95 String L_ACK_PDU::Decode ()
  96 {
  97   return "ACK";
  98 }
  99
 100 /* L_BUSY */
 101
 102 L_BUSY_PDU::L_BUSY_PDU ()
 103 {
 104 }
 105
 106 bool
 107 L_BUSY_PDU::init (const CArray & c)
 108 {
 109   if (c () != 1)
 110     return false;
 111   return true;
 112 }
 113
 114 CArray L_BUSY_PDU::ToPacket ()
 115 {
 116   uchar
 117     c = 0xC0;
 118   return CArray (&c, 1);
 119 }
 120
 121 String L_BUSY_PDU::Decode ()
 122 {
 123   return "BUSY";
 124 }
 125
 126 /* L_Unknown  */
 127
 128 L_Unknown_PDU::L_Unknown_PDU ()
 129 {
 130 }
 131
 132 bool
 133 L_Unknown_PDU::init (const CArray & c)
 134 {
 135   pdu = c;
 136   return true;
 137 }
 138
 139 CArray
 140 L_Unknown_PDU::ToPacket ()
 141 {
 142   return pdu;
 143 }
 144
 145 String
 146 L_Unknown_PDU::Decode ()
 147 {
 148   String s ("Unknown LPDU: ");
 149   unsigned i;
 150
 151   if (pdu () == 0)
 152     return "empty LPDU";
 153
 154   for (i = 0; i < pdu (); i++)
 155     addHex (s, pdu[i]);
 156
 157   return s;
 158 }
 159
 160 /* L_Busmonitor  */
 161
 162 L_Busmonitor_PDU::L_Busmonitor_PDU ()
 163 {
 164 }
 165
 166 bool
 167 L_Busmonitor_PDU::init (const CArray & c)
 168 {
 169   pdu = c;
 170   return true;
 171 }
 172
 173 CArray
 174 L_Busmonitor_PDU::ToPacket ()
 175 {
 176   return pdu;
 177 }
 178
 179 String
 180 L_Busmonitor_PDU::Decode ()
 181 {
 182   String s ("LPDU: ");
 183   unsigned i;
 184
 185   if (pdu () == 0)
 186     return "empty LPDU";
 187
 188   for (i = 0; i < pdu (); i++)
 189     addHex (s, pdu[i]);
 190   s += ":";
 191   LPDU *l = LPDU::fromPacket (pdu);
 192   s += l->Decode ();
 193   delete l;
 194   return s;
 195 }
 196
 197 /* L_Data */
 198
 199 L_Data_PDU::L_Data_PDU ()
 200 {
 201   prio = PRIO_LOW;
 202   repeated = 0;
 203   valid_checksum = 1;
 204   valid_length = 1;
 205   AddrType = IndividualAddress;
 206   source = 0;
 207   dest = 0;
 208   hopcount = 0x07;
 209 }
 210
 211 bool
 212 L_Data_PDU::init (const CArray & c)
 213 {
 214   unsigned len, i;
 215   uchar c1;
 216   if (c () < 6)
 217     return false;
 218   if ((c[0] & 0x53) != 0x10)
 219     return false;
 220   repeated = (c[0] & 0x20) ? 0 : 1;
 221   valid_length = 1;
 222   switch ((c[0] >> 2) & 0x3)
 223     {
 224     case 0:
 225       prio = PRIO_SYSTEM;
 226       break;
 227     case 1:
 228       prio = PRIO_URGENT;
 229       break;
 230     case 2:
 231       prio = PRIO_NORMAL;
 232       break;
 233     case 3:
 234       prio = PRIO_LOW;
 235       break;
 236     }
 237   if (c[0] & 0x80)
 238     {
 239       /*Standard frame */
 240       source = (c[1] << 8) | (c[2]);
 241       dest = (c[3] << 8) | (c[4]);
 242       len = (c[5] & 0x0f) + 1;
 243       hopcount = (c[5] >> 4) & 0x07;
 244       AddrType = (c[5] & 0x80) ? GroupAddress : IndividualAddress;
 245       if (len + 7 != c ())
 246         return false;
 247       data.set (c.array () + 6, len);
 248       c1 = 0;
 249       for (i = 0; i < c () - 1; i++)
 250         c1 ^= c[i];
 251       c1 = ~c1;
 252       valid_checksum = (c[c () - 1] == c1 ? 1 : 0);
 253     }
 254   else
 255     {
 256       /*extended frame */
 257       if ((c[1] & 0x0f) != 0)
 258         return false;
 259       if (c () < 7)
 260         return false;
 261       hopcount = (c[1] >> 4) & 0x07;
 262       AddrType = (c[1] & 0x80) ? GroupAddress : IndividualAddress;
 263       source = (c[2] << 8) | (c[3]);
 264       dest = (c[4] << 8) | (c[5]);
 265       len = c[6] + 1;
 266       if (len + 8 != c ())
 267         {
 268           if (c () == 23)
 269             {
 270               valid_length = 0;
 271               data.set (c.array () + 7, 8);
 272             }
 273           else
 274             return false;
 275         }
 276       else
 277         data.set (c.array () + 7, len);
 278
 279       c1 = 0;
 280       for (i = 0; i < c () - 1; i++)
 281         c1 ^= c[i];
 282       c1 = ~c1;
 283       valid_checksum = (c[c () - 1] == c1 ? 1 : 0);
 284     }
 285   return true;
 286 }
 287
 288 CArray L_Data_PDU::ToPacket ()
 289 {
 290   assert (data () >= 1);
 291   assert (data () <= 0xff);
 292   assert ((hopcount & 0xf8) == 0);
 293   CArray
 294     pdu;
 295   uchar
 296     c;
 297   unsigned
 298     i;
 299   switch (prio)
 300     {
 301     case PRIO_LOW:
 302       c = 0x3;
 303       break;
 304     case PRIO_NORMAL:
 305       c = 0x1;
 306       break;
 307     case PRIO_URGENT:
 308       c = 0x02;
 309       break;
 310     case PRIO_SYSTEM:
 311       c = 0x00;
 312       break;
 313     }
 314   if (data () - 1 <= 0x0f)
 315     {
 316       pdu.resize (7 + data ());
 317       pdu[0] = 0x90 | (c << 2) | (repeated ? 0x00 : 0x20);
 318       pdu[1] = (source >> 8) & 0xff;
 319       pdu[2] = (source) & 0xff;
 320       pdu[3] = (dest >> 8) & 0xff;
 321       pdu[4] = (dest) & 0xff;
 322       pdu[5] =
 323         (hopcount & 0x07) << 4 | ((data () - 1) & 0x0f) | (AddrType ==
 324                                                            GroupAddress ? 0x80
 325                                                            : 0x00);
 326       pdu.setpart (data.array (), 6, 1 + ((data () - 1) & 0x0f));
 327     }
 328   else
 329     {
 330       pdu.resize (8 + data ());
 331       pdu[0] = 0x10 | (c << 2) | (repeated ? 0x00 : 0x20);
 332       pdu[1] =
 333         (hopcount & 0x07) << 4 | (AddrType == GroupAddress ? 0x80 : 0x00);
 334       pdu[2] = (source >> 8) & 0xff;
 335       pdu[3] = (source) & 0xff;
 336       pdu[4] = (dest >> 8) & 0xff;
 337       pdu[5] = (dest) & 0xff;
 338       pdu[6] = (data () - 1) & 0xff;
 339       pdu.setpart (data.array (), 7, 1 + ((data () - 1) & 0xff));
 340     }
 341   /* checksum */
 342   c = 0;
 343   for (i = 0; i < pdu () - 1; i++)
 344     c ^= pdu[i];
 345   pdu[pdu () - 1] = ~c;
 346
 347   return pdu;
 348 }
 349
 350 String L_Data_PDU::Decode ()
 351 {
 352   assert (data () >= 1);
 353   assert (data () <= 0xff);
 354   assert ((hopcount & 0xf8) == 0);
 355
 356   String
 357   s ("L_Data");
 358   if (!valid_length)
 359     s += " (incomplete)";
 360   if (repeated)
 361     s += " (repeated)";
 362   switch (prio)
 363     {
 364     case PRIO_LOW:
 365       s += " low";
 366       break;
 367     case PRIO_NORMAL:
 368       s += " normal";
 369       break;
 370     case PRIO_URGENT:
 371       s += " urgent";
 372       break;
 373     case PRIO_SYSTEM:
 374       s += " system";
 375       break;
 376     }
 377   if (!valid_checksum)
 378     s += " INVALID CHECKSUM";
 379   s =
 380     s + " from " + FormatEIBAddr (source) + " to " + (AddrType ==
 381                                                       GroupAddress ?
 382                                                       FormatGroupAddr (dest) :
 383                                                       FormatEIBAddr (dest));
 384   s += " hops: ";
 385   addHex (s, hopcount);
 386   TPDU *
 387     d = TPDU::fromPacket (data);
 388   s += d->Decode ();
 389   delete
 390     d;
 391   return s;
 392 }