summaryrefslogtreecommitdiff
path: root/trunk/darwin/firmware/PIC/tools/snap_packet.cc
blob: 8ebb13dcf4b641e6742a6008dc30a112c066e95b (plain)
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
#include <iostream>
#include "snap_packet.h"
#include "snap.h"

SNAPPacket::SNAPPacket()
{
  sync = 0x54;
  hdb2 = 0x51;
  hdb1 = 0x30;
  dab = 0;
  sab = 0;
  crc = 0;
}

bool SNAPPacket::readPacket(SNAP &snap)
{
  crc = 0;
  byte actualCRC;
  try {
    for(;;) {
      sync = snap.readbyte();
      if (sync == 0x54) break;
    }
    hdb2 = computeCRC(snap.readbyte());
    hdb1 = computeCRC(snap.readbyte());
    dab = computeCRC(snap.readbyte());
    sab = computeCRC(snap.readbyte());
    for(int i = 0; i < getLength(); i++) {
      payload[i] = computeCRC(snap.readbyte());
    }
    actualCRC = snap.readbyte();
  } catch(SNAPTimeoutException) {
    std::cerr << "Timeout waiting for receive" << std::endl;
    return false;
  }
  if (crc != actualCRC) {
    std::cerr << "CRC fail, got " << (int)actualCRC << " expected " << (int)crc
	      << std::endl;
    crc = actualCRC;
    return false;
  }
  return true;
}

SNAPPacket SNAPPacket::reply()
{
  SNAPPacket p;
  p.dab = sab;
  p.sab = dab;
  return p;
}

bool SNAPPacket::send(SNAP &snap, bool verbose)
{
  for(int retries = 0; retries < 5; retries++) {
    crc = 0;
    snap.sendbyte(sync);
    snap.sendbyte(computeCRC(hdb2));
    snap.sendbyte(computeCRC(hdb1));
    snap.sendbyte(computeCRC(dab));
    snap.sendbyte(computeCRC(sab));
    for(int i = 0; i < getLength(); i++) {
      snap.sendbyte(computeCRC(payload[i]));
    }
    snap.sendbyte(crc);

    SNAPPacket p;
    if (!p.readPacket(snap)) {
      std::cerr << "Read fail 1" << std::endl;
      continue;
    }
      
    if (p.isAck()) {
      if (verbose)
	std::cout << "[ACK Received]" << std::endl;
      return true;
    }

    if (p.isNak()) {
      std::cerr << "Got NAK" << std::endl;
      continue;
    }
    std::cerr << "Read fail 2" << std::endl;
  }
  return false;
}

byte SNAPPacket::computeCRC(byte dataval)
{
  byte i = dataval ^ crc;

  crc = 0;

  if(i & 1)
    crc ^= 0x5e;
  if(i & 2)
    crc ^= 0xbc;
  if(i & 4)
    crc ^= 0x61;
  if(i & 8)
    crc ^= 0xc2;
  if(i & 0x10)
    crc ^= 0x9d;
  if(i & 0x20)
    crc ^= 0x23;
  if(i & 0x40)
    crc ^= 0x46;
  if(i & 0x80)
    crc ^= 0x8c;
  return dataval;

}