niscan/binarydata.cpp at master · rampage128/niscan · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include <arduino.h>
#include "binarydata.h"

BinaryData::BinaryData(uint8_t len) {
  _len = len;
  _payload = (char*)calloc(sizeof(char), _len+1);
}

BinaryData::~BinaryData() {
  free(_payload);
}

BinaryData::BoolResult BinaryData::readFlag(uint8_t index, unsigned char mask, unsigned char comparator) {
  struct BoolResult result;

  if (checkIndex(index) == AccessStatus::INDEXOUTOFBOUNDS) {
    result.state = AccessStatus::INDEXOUTOFBOUNDS;
    return result;
  }

  result.data = (_payload[index] & mask) == comparator;
  result.state = AccessStatus::OK;
  return result;
}

BinaryData::ByteResult BinaryData::readByte(uint8_t index) {
  struct ByteResult result;

  if (checkIndex(index) == AccessStatus::INDEXOUTOFBOUNDS) {
    result.state = AccessStatus::INDEXOUTOFBOUNDS;
    return result;
  }

  result.data = _payload[index];
  result.state = AccessStatus::OK;
  return result;
}

BinaryData::LongResult BinaryData::readLong(uint8_t index) {
  struct LongResult result;

  if (checkIndex(index + 4) == AccessStatus::INDEXOUTOFBOUNDS) {
    result.state = AccessStatus::INDEXOUTOFBOUNDS;
    return result;
  }

  result.data = (unsigned long)ntohl(*((long *)&_payload[index]));
  result.state = AccessStatus::OK;
  return result;
}

BinaryData::AccessStatus BinaryData::toggleFlag(uint8_t index, unsigned char mask) {
  if (checkIndex(index) == AccessStatus::INDEXOUTOFBOUNDS) {
    return AccessStatus::INDEXOUTOFBOUNDS;
  }

  _payload[index] ^= mask;

  return AccessStatus::OK;
}

BinaryData::AccessStatus BinaryData::writeFlag(uint8_t index, unsigned char mask, bool value) {
  if (checkIndex(index) == AccessStatus::INDEXOUTOFBOUNDS) {
    return AccessStatus::INDEXOUTOFBOUNDS;
  }

  if (value) {
    _payload[index] |= mask;
  }
  else {
    _payload[index] &= ~mask;
  }
  return AccessStatus::OK;
}

BinaryData::AccessStatus BinaryData::writeByte(uint8_t index, unsigned char value) {
  if (checkIndex(index) == AccessStatus::INDEXOUTOFBOUNDS) {
    return AccessStatus::INDEXOUTOFBOUNDS;
  }

  _payload[index] = value;
  return AccessStatus::OK;
}

BinaryData::AccessStatus BinaryData::writeData(uint8_t index, BinaryData* data, uint8_t from, uint8_t to) {
  int inputLen = to - from;

  if (checkIndex(index) == AccessStatus::INDEXOUTOFBOUNDS) {
    return AccessStatus::INDEXOUTOFBOUNDS;
  }
  if (checkIndex(index + inputLen - 1) == AccessStatus::INDEXOUTOFBOUNDS) {
    return AccessStatus::INDEXOUTOFBOUNDS;
  }

  char* inputData = data->_payload;

  for (int i = 0; i < inputLen; i++) {
    _payload[index + i] = inputData[from + i];
  }

  return AccessStatus::OK;
}

BinaryData::AccessStatus BinaryData::checkIndex(uint8_t index) {
    if (_len <= index) {
    return AccessStatus::INDEXOUTOFBOUNDS;
  }
  return AccessStatus::OK;
}

uint8_t BinaryData::getSize() {
  return _len;
}

char* BinaryData::getData() {
  return _payload;
}