cobs/zcp.c at master · Shibodd/cobs · 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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
/*
ZCP: Zero-termination and Checksum Protocol

Frame format:
DATA  [variable]
CRC16 [2]
ZERO  [1]

*/

#include <zcp.h>
#include <cobs.h>

void MMR_ZCP_Init(
    MmrZcpInstance* instance,
    uint8_t* txbuf,
    int txbuf_size,
    MmrZcpBsp bsp) {
  instance->txBuf = (RingBuffer) {
    .data = txbuf,
    .len = txbuf_size,
    .count = 0,
    .first = 0,
  };
  instance->txTransactionInProgress = false;
  instance->txTransactionSize = 0;
  instance->txTransmitting = false;
  instance->txTransmissionSize = 0;

  instance->bsp = bsp;
}

void MMR_ZCP_Run(MmrZcpInstance* instance) {
  RingBuffer* txBuf = &instance->txBuf;

  if (instance->txTransmitting) {
    if (instance->bsp.txCmplt()) {
      instance->txTransmitting = false;
      txBuf->first = ring_buf_idx(txBuf, instance->txTransmissionSize);
      txBuf->count -= instance->txTransmissionSize;
    } else {
      return;
    }
  }

  if (txBuf->count <= 0) {
    return;
  }

  int size;
  if (txBuf->first + txBuf->count > txBuf->len) {
    size = txBuf->len - txBuf->first;
  } else {
    size = txBuf->count;
  }

  if (!instance->bsp.txBegin(ring_buf_ref(txBuf, 0), size)) {
    return;
  }

  instance->txTransmitting = true;
  instance->txTransmissionSize = size;
}

bool MMR_ZCP_TxTransactionBegin(MmrZcpInstance* instance) {
  if (instance->txTransactionInProgress) {
    return false;
  }

  MmrCobsResult result = MMR_COBS_BeginEncode(&instance->txBuf, &instance->txTransactionDstIdx, &instance->txTransactionCobsIdx);
  if (result != MMR_COBS_OK) {
    return false;
  }

  instance->txTransactionSize = 0;
  instance->txTransactionInProgress = true;
  instance->bsp.crcReset();
  return true;
}

bool MMR_ZCP_TxTransactionAppend(MmrZcpInstance* instance, uint8_t* data, int len) {
  if (!instance->txTransactionInProgress) {
    return false;
  }

  MmrCobsResult result = MMR_COBS_ContinueEncode(
    data, len, &instance->txBuf,
    &instance->txTransactionDstIdx, &instance->txTransactionCobsIdx
  );
  if (result != MMR_COBS_OK) {
    return false;
  }

  instance->bsp.crcAccumulate(data, len);
  instance->txTransactionSize += len;
  return true;
}

bool MMR_ZCP_TxTransactionCommit(MmrZcpInstance* instance) {
  if (!instance->txTransactionInProgress) {
    return false;
  }

  uint16_t crc16 = instance->bsp.crcGet();
  uint8_t crc16_little_endian[sizeof(uint16_t)] = {
	  crc16 & 0xFF,
	  (crc16 >> 8) & 0xFF
  };

  // Prepare to rollback
  int old_dst_idx = instance->txTransactionDstIdx;
  int old_cobs_idx = instance->txTransactionCobsIdx;

  MmrCobsResult result;
  result = MMR_COBS_ContinueEncode(
    crc16_little_endian,
    sizeof(crc16_little_endian),
    &instance->txBuf,
    &instance->txTransactionDstIdx,
    &instance->txTransactionCobsIdx
  );
  if (result != MMR_COBS_OK) {
    return false;
  }

  result = MMR_COBS_EndEncode(&instance->txBuf, &instance->txTransactionDstIdx, &instance->txTransactionCobsIdx);
  if (result != MMR_COBS_OK) {
    instance->txTransactionDstIdx = old_dst_idx;
    instance->txTransactionCobsIdx = old_cobs_idx;
    return false;
  }
  instance->txTransactionInProgress = false;
  return true;
}

void MMR_ZCP_TxTransactionAbort(MmrZcpInstance* instance) {
  instance->txTransactionInProgress = false;
}