Subaru-TPMS-Spoofing/C_Code/tpms_receiver_raw_gcc/main.cpp at master · JoeSc/Subaru-TPMS-Spoofing · 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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
extern "C"
{
	#include "uart.h"
}
#include <stdio.h>
#include "cc1101.h"
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#define LED_DDR  DDRB
#define LED_PORT PORTB
#define LED      PINB1

CC1101 radio;
CCPACKET rx_packet;
bool new_rx_packet = false;
CCPACKET tx_packet;


#define LEN_OF_PACKET (10)
#define START_OF_DATA_OFFSET (1)
#define LEN_OF_DATA (74)

//#define enableIRQ_GDO0()          ::attachInterrupt(0, radioISR, FALLING);
//#define disableIRQ_GDO0()         ::detachInterrupt(0);

#define enableIRQ_GDO0()          EICRA = (EICRA & ~((1 << ISC00) | (1 << ISC01))) | (2 << ISC00); EIMSK |= (1 << INT0);
#define disableIRQ_GDO0()         EIMSK &= ~(1 << INT0);

ISR(INT0_vect)
{
  // Disable interrupt
  disableIRQ_GDO0();

  if (radio.rfState == RFSTATE_RX)
  {

    // Any packet waiting to be read?
    if (radio.receiveData(&rx_packet) > 0)
    {
		new_rx_packet = true;
		printf(" GOT A PACKET!!!!!!!!!!!!!!!!!!\n");
    }
  }

  // Enable interrupt
  enableIRQ_GDO0();
}


/* Take two bits and convert it to a manchester value */
uint8_t man_data_to_bin( uint8_t v0, uint8_t v1)
{
	if (( v0 == 1) && ( v1 == 0)) {
		return 0;
	}
	else if (( v0 == 0) && ( v1 == 1)) {
		return 1;
	}

	return 0xff;
}

uint64_t raw_packet_to_hex(uint8_t *packet)
{
	uint8_t v0, v1, i;
	uint64_t hex_packet = 0;

	for (i = START_OF_DATA_OFFSET; i < LEN_OF_DATA + START_OF_DATA_OFFSET; i+= 1) {
		v0 = (packet[i/8] >> (7 - (i % 8 ))) & 1;
		i += 1;
		v1 = (packet[i/8] >> (7 - (i % 8 ))) & 1;
		hex_packet = (hex_packet << 1) | man_data_to_bin(v0, v1);
	}

	return hex_packet;
}

struct tpms_packets {
   	uint8_t prepend;
	uint32_t address;
   	uint16_t pressure;
};

struct tpms_packets raw_packet_to_struct(uint8_t *packet)
{
	struct tpms_packets ret;
	uint64_t hex_pkt;

	printf("packet = ");
	for (int i = 0; i < 10 ; i+= 1)
		printf("%02x ", packet[i]);
	printf("\n");

	hex_pkt = raw_packet_to_hex(packet);

	ret.prepend = hex_pkt >> 34;
	ret.address = ((hex_pkt >> 10) & 0xffffff);
	ret.pressure = (hex_pkt & 0x3ff);

	printf("Got packet from addr = 0x%lx   prepend = %d,   pressure = %d\n", ret.address, ret.prepend, ret.pressure);

	return ret;
}


void loop() {
	uint8_t i;

	if (new_rx_packet) {
		new_rx_packet = false;

		LED_PORT ^= _BV(LED);

		printf(" rx_packet = ");
		for (i=0; i<rx_packet.length; i++)
			printf("%02x ", rx_packet.data[i]);
		printf("\n");

		struct tpms_packets tpms_packet = raw_packet_to_struct(rx_packet.data);
	}


}

int main(void) {
	uart_init();
	stdin = stdout = fdevopen(uart_putchar, uart_getchar);

	/* set LED pin as output */
	LED_DDR |= _BV(LED);
	LED_PORT &= ~_BV(LED);

	radio.init();
	printf("Initializing Radio\n");

	_delay_us(50);
	radio.setRxState();

	enableIRQ_GDO0();
	sei();


	while (1)
		loop();
	return 0;
}