Skip to content

Commit 6a746dc

Browse files
DineshDK03nashif
DineshDK03
authored and
nashif
committed
drivers: sensor: grow_r502a: add RX packet validation
Add RX packet validator where header, address, pid and checksum bytes are verified and validated on every functions. Signed-off-by: Dinesh Kumar K <[email protected]>
1 parent e08ce0e commit 6a746dc

File tree

1 file changed

+102
-53
lines changed

1 file changed

+102
-53
lines changed

‎drivers/sensor/grow_r502a/grow_r502a.c

Lines changed: 102 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,61 @@ static int transceive_packet(const struct device *dev, union r502a_packet *tx_pa
6767
return 0;
6868
}
6969

70+
static int r502a_validate_rx_packet(union r502a_packet *rx_packet)
71+
{
72+
uint16_t recv_cks = 0, calc_cks = 0;
73+
uint8_t cks_start_idx;
74+
75+
if (sys_be16_to_cpu(rx_packet->start) == R502A_STARTCODE) {
76+
LOG_DBG("startcode matched 0x%X", sys_be16_to_cpu(rx_packet->start));
77+
} else {
78+
LOG_ERR("startcode didn't match 0x%X", sys_be16_to_cpu(rx_packet->start));
79+
return -EINVAL;
80+
}
81+
82+
if (sys_be32_to_cpu(rx_packet->addr) == R502A_DEFAULT_ADDRESS) {
83+
LOG_DBG("Address matched 0x%X", sys_be32_to_cpu(rx_packet->addr));
84+
} else {
85+
LOG_ERR("Address didn't match 0x%X", sys_be32_to_cpu(rx_packet->addr));
86+
return -EINVAL;
87+
}
88+
89+
switch (rx_packet->pid) {
90+
case R502A_DATA_PACKET:
91+
LOG_DBG("Data Packet Received 0x%X", rx_packet->pid);
92+
break;
93+
case R502A_END_DATA_PACKET:
94+
LOG_DBG("End of Data Packet Received 0x%X", rx_packet->pid);
95+
break;
96+
case R502A_ACK_PACKET:
97+
LOG_DBG("Acknowledgment Packet Received 0x%X", rx_packet->pid);
98+
break;
99+
default:
100+
LOG_ERR("Error Package ID 0x%X", rx_packet->pid);
101+
return -EINVAL;
102+
}
103+
104+
cks_start_idx = sys_be16_to_cpu(rx_packet->len) - R502A_CHECKSUM_LEN;
105+
106+
recv_cks = sys_get_be16(&rx_packet->data[cks_start_idx]);
107+
108+
calc_cks += rx_packet->pid + (sys_be16_to_cpu(rx_packet->len) >> 8) +
109+
(sys_be16_to_cpu(rx_packet->len) & 0xFF);
110+
111+
for (int i = 0; i < cks_start_idx; i++) {
112+
calc_cks += rx_packet->data[i];
113+
}
114+
115+
if (recv_cks == calc_cks) {
116+
LOG_DBG("Checksum matched calculated 0x%x received 0x%x", calc_cks, recv_cks);
117+
} else {
118+
LOG_ERR("Checksum mismatch calculated 0x%x received 0x%x", calc_cks, recv_cks);
119+
return -EINVAL;
120+
}
121+
122+
return 0;
123+
}
124+
70125
static void uart_cb_tx_handler(const struct device *dev)
71126
{
72127
const struct grow_r502a_config *config = dev->config;
@@ -145,9 +200,9 @@ static int fps_set_sys_param(const struct device *dev, const struct sensor_value
145200
return ret;
146201
}
147202

148-
if (rx_packet.pid != R502A_ACK_PACKET) {
149-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
150-
return -EIO;
203+
ret = r502a_validate_rx_packet(&rx_packet);
204+
if (ret != 0) {
205+
return ret;
151206
}
152207

153208
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -180,9 +235,8 @@ int r502a_read_sys_param(const struct device *dev, struct r502a_sys_param *val)
180235
goto unlock;
181236
}
182237

183-
if (rx_packet.pid != R502A_ACK_PACKET) {
184-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
185-
ret = -EIO;
238+
ret = r502a_validate_rx_packet(&rx_packet);
239+
if (ret != 0) {
186240
goto unlock;
187241
}
188242

@@ -239,9 +293,9 @@ static int fps_led_control(const struct device *dev, struct r502a_led_params *le
239293
return ret;
240294
}
241295

242-
if (rx_packet.pid != R502A_ACK_PACKET) {
243-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
244-
return -EIO;
296+
ret = r502a_validate_rx_packet(&rx_packet);
297+
if (ret != 0) {
298+
return ret;
245299
}
246300

247301
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -273,15 +327,15 @@ static int fps_verify_password(const struct device *dev)
273327
return ret;
274328
}
275329

276-
if (rx_packet.pid != R502A_ACK_PACKET) {
277-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
278-
return -EIO;
330+
ret = r502a_validate_rx_packet(&rx_packet);
331+
if (ret != 0) {
332+
return ret;
279333
}
280334

281335
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
282336
LOG_DBG("Correct password, R502A verified");
283337
} else {
284-
LOG_ERR("Package receive error 0x%X", rx_packet.buf[R502A_CC_IDX]);
338+
LOG_ERR("Password verification error 0x%X", rx_packet.buf[R502A_CC_IDX]);
285339
return -EIO;
286340
}
287341

@@ -305,9 +359,9 @@ static int fps_get_template_count(const struct device *dev)
305359
return ret;
306360
}
307361

308-
if (rx_packet.pid != R502A_ACK_PACKET) {
309-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
310-
return -EIO;
362+
ret = r502a_validate_rx_packet(&rx_packet);
363+
if (ret != 0) {
364+
return ret;
311365
}
312366

313367
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -341,11 +395,9 @@ static int fps_read_template_table(const struct device *dev, uint32_t *free_idx)
341395
goto unlock;
342396
}
343397

344-
if (rx_packet.pid != R502A_ACK_PACKET) {
345-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
346-
ret = -EIO;
398+
ret = r502a_validate_rx_packet(&rx_packet);
399+
if (ret != 0) {
347400
goto unlock;
348-
349401
}
350402

351403
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -396,9 +448,9 @@ static int fps_get_image(const struct device *dev)
396448
return ret;
397449
}
398450

399-
if (rx_packet.pid != R502A_ACK_PACKET) {
400-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
401-
return -EIO;
451+
ret = r502a_validate_rx_packet(&rx_packet);
452+
if (ret != 0) {
453+
return ret;
402454
}
403455

404456
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -431,9 +483,9 @@ static int fps_image_to_char(const struct device *dev, uint8_t char_buf_idx)
431483
return ret;
432484
}
433485

434-
if (rx_packet.pid != R502A_ACK_PACKET) {
435-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
436-
return -EIO;
486+
ret = r502a_validate_rx_packet(&rx_packet);
487+
if (ret != 0) {
488+
return ret;
437489
}
438490

439491
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -462,9 +514,9 @@ static int fps_create_model(const struct device *dev)
462514
return ret;
463515
}
464516

465-
if (rx_packet.pid != R502A_ACK_PACKET) {
466-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
467-
return -EIO;
517+
ret = r502a_validate_rx_packet(&rx_packet);
518+
if (ret != 0) {
519+
return ret;
468520
}
469521

470522
if (rx_packet.buf[R502A_CC_IDX] == R502A_OK) {
@@ -504,9 +556,8 @@ static int fps_store_model(const struct device *dev, uint16_t id)
504556
goto unlock;
505557
}
506558

507-
if (rx_packet.pid != R502A_ACK_PACKET) {
508-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
509-
ret = -EIO;
559+
ret = r502a_validate_rx_packet(&rx_packet);
560+
if (ret != 0) {
510561
goto unlock;
511562
}
512563

@@ -546,9 +597,8 @@ static int fps_delete_model(const struct device *dev, uint16_t id, uint16_t coun
546597
goto unlock;
547598
}
548599

549-
if (rx_packet.pid != R502A_ACK_PACKET) {
550-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
551-
ret = -EIO;
600+
ret = r502a_validate_rx_packet(&rx_packet);
601+
if (ret != 0) {
552602
goto unlock;
553603
}
554604

@@ -582,9 +632,8 @@ static int fps_empty_db(const struct device *dev)
582632
goto unlock;
583633
}
584634

585-
if (rx_packet.pid != R502A_ACK_PACKET) {
586-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
587-
ret = -EIO;
635+
ret = r502a_validate_rx_packet(&rx_packet);
636+
if (ret != 0) {
588637
goto unlock;
589638
}
590639

@@ -629,9 +678,8 @@ static int fps_search(const struct device *dev, struct sensor_value *val)
629678
goto unlock;
630679
}
631680

632-
if (rx_packet.pid != R502A_ACK_PACKET) {
633-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
634-
ret = -EIO;
681+
ret = r502a_validate_rx_packet(&rx_packet);
682+
if (ret != 0) {
635683
goto unlock;
636684
}
637685

@@ -682,9 +730,8 @@ static int fps_load_template(const struct device *dev, uint16_t id)
682730
goto unlock;
683731
}
684732

685-
if (rx_packet.pid != R502A_ACK_PACKET) {
686-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
687-
ret = -EIO;
733+
ret = r502a_validate_rx_packet(&rx_packet);
734+
if (ret != 0) {
688735
goto unlock;
689736
}
690737

@@ -727,9 +774,8 @@ static int fps_match_templates(const struct device *dev, struct sensor_value *va
727774
goto unlock;
728775
}
729776

730-
if (rx_packet.pid != R502A_ACK_PACKET) {
731-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
732-
ret = -EIO;
777+
ret = r502a_validate_rx_packet(&rx_packet);
778+
if (ret != 0) {
733779
goto unlock;
734780
}
735781

@@ -814,9 +860,8 @@ int fps_upload_char_buf(const struct device *dev, struct r502a_template *temp)
814860
goto unlock;
815861
}
816862

817-
if (rx_packet.pid != R502A_ACK_PACKET) {
818-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
819-
ret = -EIO;
863+
ret = r502a_validate_rx_packet(&rx_packet);
864+
if (ret != 0) {
820865
goto unlock;
821866
}
822867

@@ -835,6 +880,11 @@ int fps_upload_char_buf(const struct device *dev, struct r502a_template *temp)
835880
goto unlock;
836881
}
837882

883+
ret = r502a_validate_rx_packet(&rx_packet);
884+
if (ret != 0) {
885+
goto unlock;
886+
}
887+
838888
memcpy(&temp->data[idx], &rx_packet.data,
839889
sys_be16_to_cpu(rx_packet.len) - R502A_CHECKSUM_LEN);
840890
idx += sys_be16_to_cpu(rx_packet.len) - R502A_CHECKSUM_LEN;
@@ -877,9 +927,8 @@ int fps_download_char_buf(const struct device *dev, uint8_t char_buf_id,
877927
goto unlock;
878928
}
879929

880-
if (rx_packet.pid != R502A_ACK_PACKET) {
881-
LOG_ERR("Error receiving ack packet 0x%X", rx_packet.pid);
882-
ret = -EIO;
930+
ret = r502a_validate_rx_packet(&rx_packet);
931+
if (ret != 0) {
883932
goto unlock;
884933
}
885934

0 commit comments

Comments
 (0)