layer7.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 "layer7.h" 21 22 #include "apdu.h" 23 24 Layer7_Broadcast::Layer7_Broadcast (TracePtr tr) 25 { 26 t = TracePtr(new Trace(*tr)); 27 t->setAuxName("L7"); 28 TRACEPRINTF (t, 5, "L7Broadcast Open"); 29 } 30 31 Layer7_Broadcast::~Layer7_Broadcast () 32 { 33 TRACEPRINTF (t, 5, "L7Broadcast Close"); 34 } 35 36 bool Layer7_Broadcast::setup(); 37 { 38 l4 = T_BroadcastPtr(new T_Broadcast (t, 0)); 39 if (!l4->setup (this)) 40 { 41 TRACEPRINTF (t, 5, "L7Broadcast init bad"); 42 l4 = 0; 43 } 44 return l4 != 0; 45 } 46 47 void 48 Layer7_Broadcast::A_IndividualAddress_Write (eibaddr_t addr) 49 { 50 A_IndividualAddress_Write_PDU a; 51 a.addr = addr; 52 l4->recv (a.ToPacket ()); 53 } 54 55 std::vector < eibaddr_t > 56 Layer7_Broadcast::A_IndividualAddress_Read (TracePtr tr, unsigned timeout) 57 { 58 std::vector < eibaddr_t > addrs; 59 A_IndividualAddress_Read_PDU r; 60 APDUPtr a; 61 l4->recv (r.ToPacket ()); 62 pth_event_t t = pth_event (PTH_EVENT_RTIME, pth_time (timeout, 0)); 63 while (pth_event_status (t) != PTH_STATUS_OCCURRED) 64 { 65 BroadcastComm *c = l4->Get (t); 66 if (c) 67 { 68 a = APDU::fromPacket (c->data, tr); 69 if (a->isResponse (&r)) 70 addrs.push_back (c->src); 71 delete a; 72 delete c; 73 } 74 } 75 pth_event_free (t, PTH_FREE_THIS); 76 return addrs; 77 } 78 79 Layer7_Connection::Layer7_Connection (TracePtr tr, eibaddr_t d) 80 { 81 t = tr; 82 TRACEPRINTF (t, 5, "L7Connection open"); 83 dest = d; 84 } 85 86 bool Layer7_Connection::setup() 87 { 88 l4 = T_ConnectionPtr(new T_Connection (t, dest)); 89 if (!l4->setup (this)) 90 { 91 TRACEPRINTF (t, 5, "L7Connection init bad"); 92 l4 = 0; 93 } 94 return l4 != 0; 95 } 96 97 void 98 Layer7_Connection::A_Restart () 99 { 100 A_Restart_PDU a; 101 l4->recv (a.ToPacket ()); 102 } 103 104 APDUPtr 105 Layer7_Connection::Request_Response (APDU * r) 106 { 107 APDUPtr a; 108 CArray *c; 109 l4->recv (r->ToPacket ()); 110 pth_event_t t = pth_event (PTH_EVENT_RTIME, pth_time (6, 100)); 111 while (pth_event_status (t) != PTH_STATUS_OCCURRED) 112 { 113 c = l4->Get (t); 114 if (!c) 115 continue; 116 if (c->size() == 0) 117 { 118 delete c; 119 continue; 120 } 121 a = APDU::fromPacket (*c, this->t); 122 delete c; 123 if (a->isResponse (r)) 124 { 125 pth_event_free (t, PTH_FREE_THIS); 126 return a; 127 } 128 } 129 pth_event_free (t, PTH_FREE_THIS); 130 return 0; 131 } 132 133 int 134 Layer7_Connection::A_Property_Read (uint8_t obj, uint8_t propertyid, 135 uint16_t start, uint8_t count, CArray & result) 136 { 137 A_PropertyValue_Read_PDU r; 138 r.obj = obj; 139 r.prop = propertyid; 140 r.start = start & 0x0fff; 141 r.count = count & 0x0f; 142 APDUPtr a = Request_Response (&r); 143 if (!a) 144 return -1; 145 A_PropertyValue_Response_PDU *a1 = (A_PropertyValue_Response_PDU *)&*a; 146 result = a1->data; 147 return 0; 148 } 149 150 int 151 Layer7_Connection::A_Property_Write (uint8_t obj, uint8_t propertyid, 152 uint16_t start, uint8_t count, 153 const CArray & data, CArray & result) 154 { 155 A_PropertyValue_Write_PDU r; 156 r.obj = obj; 157 r.prop = propertyid; 158 r.start = start & 0x0fff; 159 r.count = count & 0x0f; 160 r.data = data; 161 APDUPtr a = Request_Response (&r); 162 if (!a) 163 return -1; 164 A_PropertyValue_Response_PDU *a1 = (A_PropertyValue_Response_PDU *) a; 165 result = a1->data; 166 return 0; 167 } 168 169 int 170 Layer7_Connection::A_Property_Desc (uint8_t obj, uint8_t & property, 171 uint8_t property_index, uint8_t & type, 172 uint16_t & max_nr_elements, 173 uint8_t & access) 174 { 175 A_PropertyDescription_Read_PDU r; 176 r.obj = obj; 177 r.prop = property; 178 r.property_index = property_index; 179 APDUPtr a = Request_Response (&r); 180 if (!a) 181 return -1; 182 A_PropertyDescription_Response_PDU *a1 = 183 (A_PropertyDescription_Response_PDU *) a; 184 type = a1->type; 185 max_nr_elements = a1->count; 186 access = a1->access; 187 property = a1->prop; 188 delete a; 189 return 0; 190 } 191 192 int 193 Layer7_Connection::A_Device_Descriptor_Read (uint16_t & maskver, uint8_t type) 194 { 195 A_DeviceDescriptor_Read_PDU r; 196 r.type = type & 0x3f; 197 APDUPtr a = Request_Response (&r); 198 if (!a) 199 return -1; 200 A_DeviceDescriptor_Response_PDU *a1 = (A_DeviceDescriptor_Response_PDU *) a; 201 maskver = a1->descriptor; 202 delete a; 203 return 0; 204 } 205 206 int 207 Layer7_Connection::A_ADC_Read (uint8_t channel, uint8_t readcount, 208 int16_t & value) 209 { 210 A_ADC_Read_PDU r; 211 r.channel = channel & 0x3f; 212 r.count = readcount; 213 APDUPtr a = Request_Response (&r); 214 if (!a) 215 return -1; 216 A_ADC_Response_PDU *a1 = (A_ADC_Response_PDU *) a; 217 value = a1->val; 218 delete a; 219 return 0; 220 } 221 222 int 223 Layer7_Connection::A_Memory_Read (memaddr_t addr, uint8_t len, CArray & data) 224 { 225 A_Memory_Read_PDU r; 226 r.addr = addr; 227 r.count = len & 0x0f; 228 APDUPtr a = Request_Response (&r); 229 if (!a) 230 return -1; 231 A_Memory_Response_PDU *a1 = (A_Memory_Response_PDU *) a; 232 data = a1->data; 233 delete a; 234 return 0; 235 } 236 237 int 238 Layer7_Connection::A_Memory_Write (memaddr_t addr, const CArray & data) 239 { 240 A_Memory_Write_PDU r; 241 r.addr = addr; 242 r.count = data.size() & 0x0f; 243 r.data.set (data.data(), data.size() & 0x0f); 244 l4->recv (r.ToPacket ()); 245 return 0; 246 } 247 248 int 249 Layer7_Connection::A_Authorize (eibkey_type key, uint8_t & level) 250 { 251 A_Authorize_Request_PDU r; 252 r.key = key; 253 APDUPtr a = Request_Response (&r); 254 if (!a) 255 return -1; 256 A_Authorize_Response_PDU *a1 = (A_Authorize_Response_PDU *) a; 257 level = a1->level; 258 delete a; 259 return 0; 260 } 261 262 int 263 Layer7_Connection::A_KeyWrite (eibkey_type key, uint8_t & level) 264 { 265 A_Key_Write_PDU r; 266 r.key = key; 267 r.level = level; 268 APDUPtr a = Request_Response (&r); 269 if (!a) 270 return -1; 271 A_Key_Response_PDU *a1 = (A_Key_Response_PDU *) a; 272 level = a1->level; 273 delete a; 274 return 0; 275 } 276 277 int 278 Layer7_Connection::X_Property_Write (uint8_t obj, uint8_t propertyid, 279 uint16_t start, uint8_t count, 280 const CArray & data) 281 { 282 CArray d1; 283 if (A_Property_Write (obj, propertyid, start, count, data, d1) == -1) 284 return -1; 285 if (A_Property_Read (obj, propertyid, start, count, d1) == -1) 286 return -1; 287 if (d1 != data) 288 return -1; 289 return 0; 290 } 291 292 int 293 Layer7_Connection::X_Memory_Write (memaddr_t addr, const CArray & data) 294 { 295 CArray d1; 296 if (A_Memory_Write (addr, data) == -1) 297 return -1; 298 if (A_Memory_Read (addr, data.size(), d1) == -1) 299 return -1; 300 if (d1 != data) 301 return -2; 302 return 0; 303 } 304 305 int 306 Layer7_Connection::X_Memory_Write_Block (memaddr_t addr, const CArray & data) 307 { 308 CArray prev; 309 unsigned int i, j; 310 int k, res = 0; 311 const unsigned blocksize = 12; 312 if (X_Memory_Read_Block (addr, data.size(), prev) == -1) 313 return -1; 314 for (i = 0; i < data.size(); i++) 315 { 316 if (data[i] == prev[i]) 317 continue; 318 j = 0; 319 while (data[i + j] != prev[i + j] && j < blocksize && i + j < data.size()) 320 j++; 321 k = X_Memory_Write (addr + i, CArray (data, i, j)); 322 if (k == -1) 323 return -1; 324 if (k == -2) 325 res = -2; 326 i += j - 1; 327 } 328 329 return res; 330 } 331 332 int 333 Layer7_Connection::X_Memory_Read_Block (memaddr_t addr, unsigned int len, CArray & erg) 334 { 335 unsigned blocksize = 12; 336 CArray e; 337 erg.resize (len); 338 for (unsigned i = 0; i < len; i += blocksize) 339 { 340 rt: 341 if (A_Memory_Read 342 (addr + i, (len - i > blocksize ? blocksize : len - i), e) == -1) 343 { 344 if (blocksize == 12) 345 { 346 blocksize = 2; 347 goto rt; 348 } 349 return -1; 350 } 351 erg.setpart (e, i); 352 } 353 return 0; 354 } 355 356 int 357 Layer7_Connection::A_Memory_Write_Block (memaddr_t addr, const CArray & data) 358 { 359 CArray prev; 360 unsigned int i, j; 361 int k, res = 0; 362 const unsigned blocksize = 12; 363 364 for (i = 0; i < data.size(); i += blocksize) 365 { 366 j = blocksize; 367 if (i + j > data.size()) 368 j = data.size() - i; 369 k = A_Memory_Write (addr + i, CArray (data.data() + i, j)); 370 if (k == -1) 371 return -1; 372 } 373 374 return res; 375 } 376 377 Layer7_Individual::Layer7_Individual (TracePtr tr, eibaddr_t d) 378 { 379 t = tr; 380 TRACEPRINTF (t, 5, "L7Individual open"); 381 dest = d; 382 } 383 384 bool Layer7_Individual::setup() 385 { 386 l4 = T_IndividualPtr(new T_Individual (t, dest, false)); 387 if (!l4->setup ()) 388 { 389 TRACEPRINTF (t, 5, "L7Individual init bad"); 390 l4 = 0; 391 } 392 return l4 != 0; 393 } 394 395 APDUPtr 396 Layer7_Individual::Request_Response (APDU * r) 397 { 398 APDUPtr a; 399 CArray *c; 400 l4->recv (r->ToPacket ()); 401 pth_event_t t = pth_event (PTH_EVENT_RTIME, pth_time (6, 100)); 402 while (pth_event_status (t) != PTH_STATUS_OCCURRED) 403 { 404 c = l4->Get (t); 405 if (c) 406 { 407 if (c->size() == 0) 408 { 409 delete c; 410 pth_event_free (t, PTH_FREE_THIS); 411 return 0; 412 } 413 a = APDU::fromPacket (*c, this->t); 414 delete c; 415 if (a->isResponse (r)) 416 { 417 pth_event_free (t, PTH_FREE_THIS); 418 return a; 419 } 420 pth_event_free (t, PTH_FREE_THIS); 421 return 0; 422 } 423 } 424 pth_event_free (t, PTH_FREE_THIS); 425 return 0; 426 } 427 428 int 429 Layer7_Individual::A_Property_Read (uint8_t obj, uint8_t propertyid, 430 uint16_t start, uint8_t count, CArray & erg) 431 { 432 A_PropertyValue_Read_PDU r; 433 r.obj = obj; 434 r.prop = propertyid; 435 r.start = start & 0x0fff; 436 r.count = count & 0x0f; 437 APDUPtr a = Request_Response (&r); 438 if (!a) 439 return -1; 440 A_PropertyValue_Response_PDU *a1 = (A_PropertyValue_Response_PDU *) a; 441 erg = a1->data; 442 delete a; 443 return 0; 444 } 445 446 int 447 Layer7_Individual::A_Property_Write (uint8_t obj, uint8_t propertyid, 448 uint16_t start, uint8_t count, 449 const CArray & data, CArray & result) 450 { 451 A_PropertyValue_Write_PDU r; 452 r.obj = obj; 453 r.prop = propertyid; 454 r.start = start & 0x0fff; 455 r.count = count & 0x0f; 456 r.data = data; 457 APDUPtr a = Request_Response (&r); 458 if (!a) 459 return -1; 460 A_PropertyValue_Response_PDU *a1 = (A_PropertyValue_Response_PDU *) a; 461 result = a1->data; 462 delete a; 463 return 0; 464 }