source/libmarklin/reply.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
   5 Distributed under the GPL
   6 */
   7
   8 #include <cstring>
   9 #include <unistd.h>
  10 #include <msp/strings/formatter.h>
  11 #include "constants.h"
  12 #include "reply.h"
  13
  14 using namespace std;
  15 using namespace Msp;
  16
  17 namespace {
  18
  19 unsigned read_all(int fd, char *buf, unsigned size)
  20 {
  21         unsigned pos = 0;
  22         while(pos<size)
  23                 pos += read(fd, buf+pos, size-pos);
  24
  25         return pos;
  26 }
  27
  28 }
  29
  30 namespace Marklin {
  31
  32 Reply::Reply():
  33         err(ERR_NO_ERROR),
  34         len(0)
  35 {
  36         memset(data, 0, 128);
  37 }
  38
  39 Reply Reply::read(int fd, Cmd cmd)
  40 {
  41         Reply result;
  42
  43         char *data = reinterpret_cast<char *>(result.data);
  44
  45         if(cmd==CMD_EVENT)
  46         {
  47                 for(unsigned i=0; i<3; ++i)
  48                 {
  49                         result.len += read_all(fd, data+i, 1);
  50                         if(!(result.data[i]&0x80))
  51                                 break;
  52                 }
  53         }
  54         else if(cmd==CMD_EVENT_LOK)
  55         {
  56                 for(unsigned i=0;; i+=5)
  57                 {
  58                         result.len += read_all(fd, data+i, 1);
  59
  60                         if(result.data[i]&0x80)
  61                                 break;
  62
  63                         result.len += read_all(fd, data+i+1, 4);
  64                 }
  65         }
  66         else if(cmd==CMD_EVENT_TURNOUT)
  67         {
  68                 result.len += read_all(fd, data, 1);
  69                 result.len += read_all(fd, data+1, result.data[0]*2);
  70         }
  71         else if(cmd==CMD_EVENT_SENSOR)
  72         {
  73                 for(unsigned i=0;; i+=3)
  74                 {
  75                         result.len += read_all(fd, data+i, 1);
  76
  77                         if(result.data[i]==0)
  78                                 break;
  79
  80                         result.len += read_all(fd, data+i+1, 2);
  81                 }
  82         }
  83         else
  84         {
  85                 bool expect_errcode = (cmd!=CMD_STATUS);
  86
  87                 unsigned expected_bytes = 0;
  88                 if(cmd==CMD_STATUS || cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
  89                         expected_bytes = 1;
  90                 if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
  91                         expected_bytes = 2;
  92                 if(cmd==CMD_LOK_STATUS)
  93                         expected_bytes = 3;
  94                 if(cmd==CMD_LOK_CONFIG)
  95                         expected_bytes = 4;
  96
  97                 if(expect_errcode)
  98                 {
  99                         char c;
 100                         read_all(fd, &c, 1);
 101                         result.err = static_cast<Error>(c);
 102                 }
 103
 104                 if(result.err==ERR_NO_ERROR)
 105                         result.len += read_all(fd, data, expected_bytes);
 106         }
 107
 108         return result;
 109 }
 110
 111 ostream &operator<<(ostream &out, const Reply &reply)
 112 {
 113         out<<reply.err;
 114         for(unsigned i=0; i<reply.len; ++i)
 115                 out<<format(" %02X", static_cast<int>(reply.data[i]));
 116
 117         return out;
 118 }
 119
 120 } // namespace Marklin