diff --git a/serial_link/protocol/frame_validator.c b/serial_link/protocol/frame_validator.c index 1ffd3aad8..d3337f6e3 100644 --- a/serial_link/protocol/frame_validator.c +++ b/serial_link/protocol/frame_validator.c @@ -23,6 +23,7 @@ SOFTWARE. */ #include "protocol/frame_validator.h" +#include "protocol/frame_router.h" const uint32_t poly8_lookup[256] = { @@ -101,5 +102,11 @@ static uint32_t crc32_byte(uint8_t *p, uint32_t bytelength) } void recv_frame(uint8_t* data, uint16_t size) { - + if (size > 4) { + uint32_t frame_crc = *(uint32_t*)(data + size - 4); + uint32_t expected_crc = crc32_byte(data, size - 4); + if (frame_crc == expected_crc) { + route_frame(data, size-4); + } + } } diff --git a/serial_link/tests/frame_validator_tests.c b/serial_link/tests/frame_validator_tests.c index f6a4fcd84..1aca9f95f 100644 --- a/serial_link/tests/frame_validator_tests.c +++ b/serial_link/tests/frame_validator_tests.c @@ -42,3 +42,38 @@ Ensure(FrameValidator, doesnt_validate_frames_under_5_bytes) { recv_frame(data, 3); recv_frame(data, 4); } + +Ensure(FrameValidator, validates_one_byte_frame_with_correct_crc) { + uint8_t data[] = {0x44, 0x04, 0x6A, 0xB3, 0xA3}; + expect(route_frame, + when(size, is_equal_to(1)), + when(data, is_equal_to_contents_of(data, 1)) + ); + recv_frame(data, 5); +} + +Ensure(FrameValidator, does_not_validate_one_byte_frame_with_incorrect_crc) { + uint8_t data[] = {0x44, 0, 0, 0, 0}; + never_expect(route_frame); + recv_frame(data, 5); +} + +Ensure(FrameValidator, validates_four_byte_frame_with_correct_crc) { + uint8_t data[] = {0x44, 0x10, 0xFF, 0x00, 0x74, 0x4E, 0x30, 0xBA}; + expect(route_frame, + when(size, is_equal_to(4)), + when(data, is_equal_to_contents_of(data, 4)) + ); + recv_frame(data, 8); +} + +Ensure(FrameValidator, validates_five_byte_frame_with_correct_crc) { + //0xBA304E74 + //0x470B99F4 + uint8_t data[] = {1, 2, 3, 4, 5, 0xF4, 0x99, 0x0B, 0x47}; + expect(route_frame, + when(size, is_equal_to(5)), + when(data, is_equal_to_contents_of(data, 5)) + ); + recv_frame(data, 9); +}