3 files changed, 60 insertions, 9 deletions

diff --git a/main.c b/main.c
index 5f51edb..858034f 100644
--- a/main.c
+++ b/main.c
@@ -33,14 +33,30 @@ static inline void uart_write(const char *s)
 
 int main(void)
 {
+	struct fpm_cfg cfg;
+
 	cli();
 	Soft_UART_init();
 	bit_set(DDRB,5);
 	sei();
 	
 	fpm_init();
+	fpm_get_cfg(&cfg);
+	fpm_clear_db();
+
 	if (fpm_get_count() == 0) {
-		fpm_enroll();
+		// todo: check againstr capacity in prod
+		if (fpm_enroll(1)) {
+			fpm_led_on(BLUE);
+			_delay_ms(500);
+			fpm_led_off();
+			_delay_ms(500);
+		} else {
+			fpm_led_on(RED);
+			_delay_ms(500);
+			fpm_led_off();
+			_delay_ms(500);
+		}
 	}
 
     while (1) 
diff --git a/r503.c b/r503.c
index 6f8a259..f78b0ac 100644
--- a/r503.c
+++ b/r503.c
@@ -163,6 +163,18 @@ static inline uint8_t scan(void)
 	return buf[0] == OK;
 }
 
+static inline uint8_t img2tz(uint8_t bufid)
+{
+	uint16_t n;
+	uint8_t buf[MAXPDLEN];
+	
+	buf[0] = 0x02;	
+	buf[1] = bufid;
+	send(0x01, buf, 2);
+	recv(buf, &n);
+	return buf[0] == OK;
+}
+
 void fpm_led_on(COLOR color)
 {
 	led_ctrl(0x03, color);
@@ -245,14 +257,37 @@ uint16_t fpm_get_count(void)
 	return count;
 }
 
-uint8_t fpm_enroll(void)
+uint8_t fpm_enroll(uint16_t id)
 {
-	uint16_t n, retries;
-	uint8_t buf[MAXPDLEN], led;
+	uint16_t n;
+	uint8_t buf[MAXPDLEN];
+
+	if (!scan())
+		return 0;
+
+	if (!img2tz(1))
+		return 0;
 	
-	if (scan())	{
-		
-	}
+	_delay_ms(2000);
 
-	return 0;
+	if (!scan())
+		return 0;
+
+	if (!img2tz(2))
+		return 0;
+
+	buf[0] = 0x05;
+	send(0x01, buf, 1);
+	recv(buf, &n);
+	if (buf[0] != OK)
+		return 0;
+
+	buf[0] = 0x06;
+	buf[1] = 1;
+	buf[2] = (uint8_t)(id >> 8);
+	buf[3] = (uint8_t)(id & 0xFF);
+	send(0x01, buf, 4);
+	recv(buf, &n);
+
+	return buf[0] == OK;
 }
diff --git a/r503.h b/r503.h
index 5582552..9cf4909 100644
--- a/r503.h
+++ b/r503.h
@@ -31,6 +31,6 @@ uint8_t fpm_clear_db(void);
 
 uint16_t fpm_get_count(void);
 
-uint8_t fpm_enroll(void);
+uint8_t fpm_enroll(uint16_t id);
 
 #endif /* FPM_R50_H */