Merge branch 'firmware_upgrade' into 'master'HEADmaster

Firmware upgrade

See merge request o-3000/driver!1

1 files changed, 495 insertions, 0 deletions

diff --git a/o3000_upgrade.c b/o3000_upgrade.c
new file mode 100644
index 0000000..9e461e8
--- /dev/null
+++ b/o3000_upgrade.c
@@ -0,0 +1,495 @@
+/**
+* @file			o3000_upgrade.c
+* @brief		O-3000 firmware upgrade
+* @author		Patrick Roth - roth@stettbacher.ch
+* @author		Patrick Brunner - brunner@stettbacher.ch
+* @author		Sophia Papagiannaki - papagiannaki@stettbacher.ch
+* @version		0.1
+* @date			2020-05-12
+* @copyright	2020 Stettbacher Signal Processing AG
+* 
+* @remarks
+*
+* <PRE>
+* This library is free software; you can redistribute it and/or
+* modify it under the terms of the GNU Lesser General Public
+* License as published by the Free Software Foundation; either
+* version 2.1 of the License, or (at your option) any later version.
+*
+* This library is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+* Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public
+* License along with this library; if not, write to the Free Software
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+* </PRE>
+*
+*/
+
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <libusb-1.0/libusb.h>
+
+#include "o3000_upgrade.h"
+#include "md5_helper.h"
+#include "o3000.h"
+
+#define BUF_LEN				256
+#define TRANSFER_SIZE		512
+#define MAX_RETRIES			5
+
+#define BL_OUT_EP			(2)
+#define XML_IN_EP			(2 | LIBUSB_ENDPOINT_IN)
+#define XML_OUT_EP			(1)
+
+// table of valid bootloader ids
+const char* bl_ids[] = {
+	"Streaming Camera Bootloader in HS mode", 	// version without proper product id
+	"Camera O-30xx (HS mode)", 					// version proper product id
+};
+
+// table of valid application ids
+const char* app_ids[] = {
+	"Streaming Camera in HS mode", 	// version without proper product id (LEGACY)
+	"Camera O-3010 (HS mode)", 		// rolling shutter, mono version     (LEGACY)
+	"Camera O-3020 (HS mode)", 		// rolling shutter, color version    (LEGACY)
+	"Camera O-3010", 				// rolling shutter, mono version (without speed mode)
+	"Camera O-3020", 				// rolling shutter, color version (without speed mode)
+	"Camera O-3110", 				// global shutter, mono version
+	"Camera O-3120", 				// global shutter, color version
+};
+
+static struct cam_usb_t {
+	struct libusb_context 		*ctx;				// libusb context
+	struct libusb_device_handle *dev_handle;		// the USB device handle
+	struct libusb_device		*dev;				// the USB device
+	struct libusb_device_descriptor desc;
+	uint32_t					transfer_size;
+	struct libusb_transfer 		*transfer_data_out;	///< libusb specific transfer structure
+	struct libusb_transfer 		*transfer_xml_out;	///< libusb specific transfer structure
+	struct libusb_transfer 		*transfer_xml_in;	///< libusb specific transfer structure
+	int							data_buf_size;		///< image data buffer size in bytes
+} cam_usb;
+
+enum cam_mode_t {
+	CM_UNKNOWN = 0,
+	CM_BOOTLOADER,
+	CM_APPLICATION,
+};
+
+
+volatile static bool xml_out_xfer_done;
+volatile static bool data_out_xfer_done;
+
+/**
+ * @brief  Transfer done callback for XML out
+ *
+ * Note: Transfer done only means submitted, not actually completed!
+ * @param  transfer Pointer to XML transfer which has been submitted
+ */
+static void LIBUSB_CALL cb_xml_out_xfer_done(struct libusb_transfer *transfer)
+{
+	if (transfer->status != LIBUSB_TRANSFER_COMPLETED) {
+		printf("Error: %s: XML out transfer failed with status %d.\n",__func__, transfer->status);
+	}
+	xml_out_xfer_done = true;
+}
+
+/**
+ * @brief  Transfer done callback for data out
+ *
+ * Note: Transfer done only means submitted, not actually completed!
+ * @param  transfer Pointer to XML transfer which has been submitted
+ */
+static void LIBUSB_CALL cb_data_out_xfer_done(struct libusb_transfer *transfer)
+{
+	if (transfer->status != LIBUSB_TRANSFER_COMPLETED) {
+		printf("Error: %s: Data out transfer failed with status %d.\n",__func__, transfer->status);
+	}
+	data_out_xfer_done = true;
+}
+
+/**
+ * Wait until XML transfer has finished.
+ */
+static void wait_for_xml_xfer_done(void)
+{
+	while(!xml_out_xfer_done) {
+		libusb_handle_events(cam_usb.ctx);
+	};
+}
+
+/**
+ * 	Wait until data transfer has finished.
+ */
+static void wait_for_data_xfer_done(void)
+{
+	while(!data_out_xfer_done) {
+		libusb_handle_events(cam_usb.ctx);
+	};
+}
+
+/**
+ * @brief  Send XML packet.
+ *
+ * Note: this function accesses the global structure cam_usb! 
+ * @param  msg Message content
+ * @param  msg_len Message content length
+ * @retval 0: success, otherwise: error
+ */
+static int send_xml(char *msg, int msg_len)
+{
+	int retval;
+
+	cam_usb.transfer_xml_out = libusb_alloc_transfer(0);
+	if (!cam_usb.transfer_xml_out)
+		return -1;
+
+	// throw away transfer after use
+	cam_usb.transfer_xml_out->flags = LIBUSB_TRANSFER_FREE_TRANSFER;
+
+	libusb_fill_bulk_transfer(cam_usb.transfer_xml_out, cam_usb.dev_handle, XML_OUT_EP, (unsigned char*)msg,
+		msg_len, cb_xml_out_xfer_done, (unsigned char*)msg, 0);
+
+	retval = libusb_submit_transfer(cam_usb.transfer_xml_out);
+	if(retval) {
+		printf("Error: %s: Failed to send XML packet.\n", __func__);
+		return -1;
+	}
+	return 0;
+}
+
+/**
+ * @brief  Send binary packet.
+ *
+ * Note: this function accesses the global structure cam_usb!
+ * @param  data_buf data buffer
+ * @param  data_len data length
+ * @retval 0: success, otherwise: error
+ */
+static int send_data(unsigned char *data_buf, int data_len)
+{
+	int retval;
+
+	cam_usb.transfer_data_out = libusb_alloc_transfer(0);
+	if (!cam_usb.transfer_data_out)
+		return -1;
+
+	// throw away transfer after use
+	cam_usb.transfer_data_out->flags = LIBUSB_TRANSFER_FREE_TRANSFER;
+
+	libusb_fill_bulk_transfer(cam_usb.transfer_data_out, cam_usb.dev_handle, BL_OUT_EP, (unsigned char*)data_buf,
+		data_len, cb_data_out_xfer_done, (unsigned char*)data_buf, 0);
+
+	retval = libusb_submit_transfer(cam_usb.transfer_data_out);
+	if(retval) {
+		printf("Error: %s: Failed to send binary data packet.\n", __func__);
+		return -1;
+	}
+	return 0;
+}
+
+
+/**
+ * 
+ * 	Open libusb.
+ * 
+ * 	Note: this function accesses the global structure cam_usb!
+ *	@retval Camera mode (see @ref cam_mode_t)
+ * 
+ */
+static enum cam_mode_t usb_open(void) 
+{
+	int r,i;
+	unsigned char buf[BUF_LEN];
+	enum cam_mode_t mode = CM_UNKNOWN;
+	int retries = MAX_RETRIES;
+	int num_bl_ids;
+	int num_app_ids;
+
+	if (libusb_init(&cam_usb.ctx) < 0) {
+		printf("Error: %s: Failed to initialise libusb.\n", __func__);
+		return mode;
+	}
+
+	// setting up USB logging. That's tricky, as LOG_LEVEL_INFO prints (almost) no log messages,
+	// whereas the next more verbose level LOG_LEVEL_DEBUG will flood the terminal
+	libusb_set_debug(cam_usb.ctx, LIBUSB_LOG_LEVEL_INFO);
+	
+	// If the camera has just performed a reset, it might not be ready yet.
+	// We try MAX_RETRIES times, before we give up.
+	while(retries > 0) {
+		if((cam_usb.dev_handle = libusb_open_device_with_vid_pid(cam_usb.ctx, O3000_VID, O3000_PID)) == NULL) {
+			printf("retry: %d\n",retries);
+			retries--;
+			sleep(1);
+		} else {
+			break;
+		}
+	}
+
+	if(retries <= 0) {
+		printf("Error: %s: Could not find/open device after %d retries\nAre you root?\n", __func__, MAX_RETRIES);
+		libusb_exit(cam_usb.ctx);
+		return mode;
+	}
+	
+	if((r = libusb_claim_interface(cam_usb.dev_handle, 0)) < 0) {
+		printf("Error: %s: usb_claim_interface error %d.\n", __func__, r);
+		libusb_close(cam_usb.dev_handle);
+		libusb_exit(cam_usb.ctx);
+		return mode;
+	}
+
+	cam_usb.dev = libusb_get_device(cam_usb.dev_handle);
+
+	r = libusb_get_device_descriptor (cam_usb.dev, &cam_usb.desc);
+	if(r) {
+		printf("Error: %s: libusb_get_device_descriptor error %d.\n", __func__, r);
+	}
+
+	// determine camera mode
+	r = libusb_get_string_descriptor_ascii(cam_usb.dev_handle, cam_usb.desc.iProduct, buf, BUF_LEN);
+	if(r > 0) {
+	
+		// get number of app/bootloader ids
+		num_app_ids = sizeof(app_ids)/sizeof(app_ids[0]);
+		num_bl_ids 	= sizeof(bl_ids)/sizeof(bl_ids[0]);
+
+		// look for bootloader ids
+		for(i=0; i<num_bl_ids; i++) {
+			if(!strcmp((char*)buf, bl_ids[i])) {
+				// found a valid bootloader id
+				mode = CM_BOOTLOADER;
+			}
+		}
+
+		if(mode == CM_UNKNOWN) {
+			// look for app ids
+			for(i=0; i<num_app_ids; i++) {
+				if(!strcmp((char*)buf, app_ids[i])) {
+					// found a valid app id
+					mode = CM_APPLICATION;
+				}
+			}
+		}
+			
+		if(mode == CM_UNKNOWN) {
+			printf("Error: %s: Could not make sense out of product %s.\n", __func__, buf);
+		}
+	
+	} else {
+		printf("Error: %s: libusb_get_string_descriptor_ascii error %d.\n", __func__, r);
+	}
+
+	return mode;
+}
+
+/**
+ * Close usb and exit connection.
+ */
+static void usb_close(void)
+{
+	libusb_release_interface(cam_usb.dev_handle, 0);
+	libusb_close(cam_usb.dev_handle);
+	libusb_exit(cam_usb.ctx);
+	
+	// although
+	// the usb_close/usb_open cycle does not seem to work properly
+	// without a short pause. Without the pause, usb_open returns
+	// a cam_mode which in fact is the previous one.
+	sleep(1);
+}
+
+/**
+ * Force camera to start in upgrade mode.
+ * 
+ * @return camera mode on starting (see @ref cam_mode_t)
+ */
+static enum cam_mode_t force_into_upgrade_mode() {
+
+	char 	msg[BUF_LEN];
+	enum cam_mode_t cam_mode = CM_UNKNOWN;
+	
+	snprintf(msg, BUF_LEN,
+	"<camera>" \
+		"<upgrade> %s </upgrade>" \
+	"</camera>",
+	"application");
+	
+	xml_out_xfer_done = false;
+	send_xml(msg, strlen(msg));
+	wait_for_xml_xfer_done();
+	
+	usb_close();
+	cam_mode = usb_open();
+	
+	return cam_mode;
+}
+
+/**
+ * Create and send the firmware update package information.
+ * 
+ * @param data pointer to firmware update data
+ * @param data_len firmware update data lenght in bytes
+ */
+static void create_and_send_update_info(unsigned char *data, int data_len) {
+	
+	char 	msg[BUF_LEN];
+	char 	md5[33];
+	
+	// compute MD5 digest
+	getMd5ChecksumString(data, data_len, md5);
+	
+	snprintf(msg, BUF_LEN,
+		"<camera>" \
+			"<upgrade_info>" \
+				"<section>%s</section>" \
+				"<size>%d</size>" \
+				"<checksum>%s</checksum>" \
+			"</upgrade_info>" \
+		"</camera>",
+		"application", data_len, md5);
+	
+	xml_out_xfer_done = false;
+	send_xml(msg, strlen(msg));
+	wait_for_xml_xfer_done();
+}
+
+/**
+ * Send the firmware update package data, close and
+ * open again usb connection.
+ * 
+ * @param data pointer to firmware update data
+ * @param data_len firmware update data lenght in bytes
+ * @return camera mode (see @ref cam_mode_t)
+ */
+static enum cam_mode_t send_update_data(unsigned char *data, int data_len) {
+
+	int		num_chunks;
+	int		last_chunk_size;
+	enum cam_mode_t cam_mode = CM_UNKNOWN;
+	
+	// calculate number of chunks to be sent and size of last chunk
+	num_chunks = data_len / TRANSFER_SIZE;
+	last_chunk_size = data_len - (num_chunks * TRANSFER_SIZE);
+
+	// send complete chunks
+	for(int i = 0; i < num_chunks; i++) {
+		data_out_xfer_done = false;
+		send_data(data + i*TRANSFER_SIZE, TRANSFER_SIZE);
+		wait_for_data_xfer_done();
+	}
+
+	// send incomplete chunk (if available)
+	if(last_chunk_size > 0) {
+		data_out_xfer_done = false;
+		send_data(data + num_chunks*TRANSFER_SIZE, last_chunk_size);
+		wait_for_data_xfer_done();
+	}
+	
+	usb_close();
+	cam_mode = usb_open();
+	
+	return cam_mode;
+}
+
+/**
+ * Try MAX_RETRIES to open camera and check if
+ * it started in bootloader mode.
+ * 
+ * @return camera mode (see @ref cam_mode_t)
+ */
+static enum cam_mode_t wait_for_bootloader_mode() {
+
+	int i;
+	enum cam_mode_t cam_mode = CM_UNKNOWN;
+	
+	for (i = 0; i < MAX_RETRIES; i++){
+
+		usb_close();
+
+		cam_mode = usb_open();
+		
+		if(cam_mode != CM_BOOTLOADER) {
+			continue;		// try again
+		}
+		break;	// started in bootloader mode so exit loop and return
+	}
+	
+	return cam_mode;
+}
+
+/**
+ * Do firmware upgrade of the device.
+ *  
+ * @param data pointer to firmware update data binary
+ * @param data_len firmware update data lenght in bytes
+ * @return 0 on success, error code on failure
+ */
+int firmware_upgrade(unsigned char *data, int data_len) {
+
+	enum cam_mode_t cam_mode = CM_UNKNOWN;
+	
+	cam_mode = usb_open();
+	
+	if(cam_mode == CM_UNKNOWN) {
+		printf("Error: %s: Failed to initialise usb.\n", __func__);
+		return O3000_ERROR_USB_INIT_FAILURE;
+	}
+	
+	/**
+	 * If camera started in application mode,
+	 * it has to be forced into upgrade mode.
+	 */
+	if (cam_mode == CM_APPLICATION) {
+		cam_mode = force_into_upgrade_mode();
+	}
+	
+	/**
+	 * Now camera should start in bootloader mode.
+	 * If not, try MAX_RETRIES times. If still
+	 * not in bootloader mode, return error code.
+	 */
+	if (cam_mode != CM_BOOTLOADER) {
+		
+		cam_mode = wait_for_bootloader_mode();
+		
+		if (cam_mode != CM_BOOTLOADER) {
+			printf("Error: Timeout! Camera won't start in bootloader mode\n");
+			usb_close();
+			return O3000_ERROR_BOOTLOADER_TIMEOUT;
+		}
+	}
+	
+	/**
+	 * Camera is finally in bootloader mode, ready
+	 * for the upgrade. So, send xml package first
+	 * and then binary data.
+	 */
+	create_and_send_update_info(data, data_len);
+
+	cam_mode = send_update_data(data, data_len);	
+	
+	usb_close();
+	
+	if(cam_mode == CM_APPLICATION) {
+		printf("Camera upgrade successful.\n");
+	}
+	else if (cam_mode == CM_BOOTLOADER) {
+		printf("Error: %s: Camera is still in bootloader mode.\n", __func__);
+		return O3000_ERROR_OTHER;
+	}
+	else if (cam_mode == CM_UNKNOWN) {
+		printf("Error: %s: Failed to initialise usb.\n", __func__);
+		return O3000_ERROR_USB_INIT_FAILURE;
+	}
+
+	return 0;
+	
+}