X-Git-Url: https://gerrit.automotivelinux.org/gerrit/gitweb?a=blobdiff_plain;f=ll-can-binding.c;h=8f96d7b4dec90dce9979367ab12ed7bc5799ee88;hb=dad2f99bbbea4ece13b0eaa2b4edeb37bb50c199;hp=ae748b9df04c1c381931a7dcaec6728e6920faee;hpb=9650f4238498e09230ff76bf458043be9ef00df2;p=apps%2Fagl-service-can-low-level.git

diff --git a/ll-can-binding.c b/ll-can-binding.c
index ae748b9d..8f96d7b4 100644
--- a/ll-can-binding.c
+++ b/ll-can-binding.c
@@ -39,6 +39,7 @@
 #include <afb/afb-service-itf.h>
 
 #include "ll-can-binding.h"
+#include "obd2.h"
 
 /*************************************************************************/
 /*************************************************************************/
@@ -97,7 +98,7 @@ static int socket_test()
  *
  * return can_event
  */
-static can_event *get_event_of_id(uint32_t id)
+static can_event *get_event_list_of_id(uint32_t id)
 {
 	can_event *current;
 
@@ -154,19 +155,18 @@ static char* create_name(uint32_t id)
  *  object.
  *
  *  return : json object
- *
- *
  */
 static json_object* create_json_from_openxc_CanMessage(event *event)
 {
 	struct json_object *json;
-	openxc_CanMessage can_message;
 
 	/*
 	 * TODO: process the openxc_CanMessage struct. Should be a call to a
 	 * decoder function relative to that msg
-	 */
+
+	openxc_CanMessage can_message;
 	can_message = event->can_message;
+	 */
 
 	json = json_object_new_object();
 	json_object_object_add(json, "name", json_object_new_string(event->name));
@@ -269,6 +269,79 @@ static int write_can()
 	return rc;
 }
 
+/*
+ * Parse the CAN frame data payload as a CAN packet
+ * TODO: parse as an OpenXC Can Message. Don't translate as ASCII and put bytes
+ * directly into openxc_CanMessage
+ */
+static int parse_can_frame(openxc_CanMessage *can_message, struct canfd_frame *canfd_frame, int maxdlen)
+{
+	int i, len;
+	//size_t n_msg;
+
+	len = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
+
+	can_message->has_id = true;
+	if (canfd_frame->can_id & CAN_ERR_FLAG)
+		can_message->id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
+	else if (canfd_frame->can_id & CAN_EFF_FLAG)
+	{
+		can_message->has_frame_format = true;
+		can_message->frame_format = openxc_CanMessage_FrameFormat_EXTENDED;
+		can_message->id = canfd_frame->can_id & CAN_EFF_MASK;
+	} else
+	{
+		can_message->has_frame_format = true;
+		can_message->frame_format = openxc_CanMessage_FrameFormat_STANDARD;
+		can_message->id = canfd_frame->can_id & CAN_SFF_MASK;
+	}
+
+	/* Don't know what to do with that for now as we haven't
+	 * len fields in openxc_CanMessage struct
+
+	 * standard CAN frames may have RTR enabled. There are no ERR frames with RTR
+	if (maxdlen == CAN_MAX_DLEN && canfd_frame->can_id & CAN_RTR_FLAG)
+	{
+		// print a given CAN 2.0B DLC if it's not zero
+		if (canfd_frame->len && canfd_frame->len <= CAN_MAX_DLC)
+			buf[offset++] = hex_asc_upper[canfd_frame->len & 0xF];
+
+		buf[offset] = 0;
+		return NULL;
+	}
+	*/
+
+	/* Doesn't handle real canfd_frame for now
+	if (maxdlen == CANFD_MAX_DLEN)
+	{
+		// add CAN FD specific escape char and flags
+		canfd_frame->flags & 0xF;
+	} */
+
+	if (sizeof(canfd_frame->data) <= sizeof(can_message->data.bytes))
+	{
+		for (i = 0; i < len; i++)
+			can_message->data.bytes[i] = canfd_frame->data[i];
+		return 0;
+	} else if (sizeof(canfd_frame->data) <= CAN_MAX_DLEN)
+	{
+		ERROR(interface, "parse_can_frame: can_frame data too long to be stored into openxc_CanMessage data field");
+		return -1;
+		/* TODO create as many as needed openxc_CanMessage into an array to store all data from canfd_frame
+		n_msg = CAN_MAX_DLEN / sizeof(canfd_frame->data.bytes);
+		for (i = 0; i < len; i++)
+			can_message->data.bytes[i] = canfd_frame->data[i]; */
+	} else
+	{
+		ERROR(interface, "parse_can_frame: can_frame is really too long here. Size of data greater than canfd maximum 64bytes size. Is it a CAN message ?");
+		return -2;
+	}
+
+	/* You should not reach this return statement */
+	return -3;
+}
+
+
 /*
  * Read on CAN bus and return how much bytes has been read.
  */
@@ -278,7 +351,7 @@ static int read_can(openxc_CanMessage *can_message)
 	int maxdlen;
 
 	/* Test that socket is really opened */
-	if ( socket_test < 0)
+	if ( socket_test() < 0)
 	{
 		if (retry(open_can_dev) < 0)
 		{
@@ -316,64 +389,15 @@ static int read_can(openxc_CanMessage *can_message)
 		return -3;
 	}
 
-	parse_can_frame(can_message, &canfd_frame, maxdlen);
-}
-
-/*
- * Parse the CAN frame data payload as a CAN packet
- * TODO: parse as an OpenXC Can Message. Don't translate as ASCII and put bytes
- * directly into openxc_CanMessage
- */
-static void parse_can_frame(openxc_CanMessage *can_message, struct canfd_frame *canfd_frame, int maxdlen)
-{
-	int i,offset;
-	int len = (canfd_frame->len > maxdlen) ? maxdlen : canfd_frame->len;
-
-	can_message->has_id = true;
-	if (canfd_frame->can_id & CAN_ERR_FLAG)
-		can_message->id = canfd_frame->can_id & (CAN_ERR_MASK|CAN_ERR_FLAG);
-	else if (canfd_frame->can_id & CAN_EFF_FLAG)
-	{
-		can_message->has_frame_format = true;
-		can_message->frame_format = openxc_CanMessage_FrameFormat_EXTENDED;
-		can_message->id = canfd_frame->can_id & CAN_EFF_MASK;
-	} else
-	{
-		can_message->has_frame_format = true;
-		can_message->frame_format = openxc_CanMessage_FrameFormat_STANDARD;
-		can_message->id = canfd_frame->can_id & CAN_SFF_MASK;
-	}
-
-	/* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */
-	if (maxdlen == CAN_MAX_DLEN && canfd_frame->can_id & CAN_RTR_FLAG)
-	{
-		/* Don't know what to do with that for now as we haven't 
-		 * len fields in openxc_CanMessage struct
-		 *
-		 * print a given CAN 2.0B DLC if it's not zero
-		if (canfd_frame->len && canfd_frame->len <= CAN_MAX_DLC)
-			buf[offset++] = hex_asc_upper[canfd_frame->len & 0xF];
-
-		buf[offset] = 0;*/
-		return;
-	}
-
-	if (maxdlen == CANFD_MAX_DLEN)
+	if (parse_can_frame(can_message, &canfd_frame, maxdlen))
 	{
-		/* add CAN FD specific escape char and flags */
-		canfd_frame->flags & 0xF;
-	}
-
-	for (i = 0; i < len; i++)
-	{
-		//put_hex_byte(buf + offset, canfd_frame->data[i]);
-		//offset += 2;
+		ERROR(interface, "read_can: Can't parse the can frame. ID: %i, DLC: %i, DATA: %s", 
+		      canfd_frame.can_id, canfd_frame.len, canfd_frame.data);
+		return -4;
 	}
 
-//	buf[offset] = 0;
-	return;
+	return 0;
 }
-
 /*************************************************************************/
 /*************************************************************************/
 /**									**/
@@ -383,53 +407,38 @@ static void parse_can_frame(openxc_CanMessage *can_message, struct canfd_frame *
 /**									**/
 /*************************************************************************/
 /*************************************************************************/
+static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata);
+
 /*
- * called on an event on the CAN bus
+ * Get the event loop running.
+ * Will trigger on_event function on EPOLLIN event on socket
+ *
+ * Return 0 or positive value on success. Else negative value for failure.
  */
-static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
+static int connect_to_event_loop()
 {
-	openxc_CanMessage can_message;
+	sd_event *event_loop;
+	sd_event_source *source;
+	int rc;
 
-	/* read available data */
-	if ((revents & EPOLLIN) != 0)
+	if (can_handler.socket < 0)
 	{
-		read_can(&can_message);
-		send_event();
+		return can_handler.socket;
 	}
 
-	/* check if error or hangup */
-	if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0)
+	event_loop = afb_daemon_get_event_loop(interface->daemon);
+	rc = sd_event_add_io(event_loop, &source, can_handler.socket, EPOLLIN, on_event, NULL);
+	if (rc < 0)
 	{
-		sd_event_source_unref(s);
-		close(fd);
-		connect_to_event_loop();
+		close(can_handler.socket);
+		ERROR(interface, "Can't connect CAN bus %s to the event loop", can_handler.device);
+	} else
+	{
+		NOTICE(interface, "Connected CAN bus %s to the event loop", can_handler.device);
 	}
 
-	return 0;
-}
-
-/*
- * get or create an event handler for the type
- * TODO: implement function and handle retrieve or create an event as needed
- */
-static event *get_event(uint32_t id, enum type type)
-{
-	event *event;
-	can_event *list;
-
-	/* find the can list by id */
-	list = get_event_of_id(id);
-
-	/* make the new event */
-	event = (can_event*)calloc(1, sizeof(can_event));
-	event->next = event;
-	list->events = event;
-	event->name = create_name(id);
-	event->afb_event = afb_daemon_make_event(interface->daemon, event->name);
-
-	return event;
+	return rc;
 }
-
 /*
  * Send all events
  */
@@ -456,34 +465,51 @@ static void send_event()
 }
 
 /*
- * Get the event loop running.
- * Will trigger on_event function on EPOLLIN event on socket
- *
- * Return 0 or positive value on success. Else negative value for failure.
+ * called on an event on the CAN bus
  */
-static int connect_to_event_loop()
+static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
 {
-	sd_event *event_loop;
-	sd_event_source *source;
-	int rc;
+	openxc_CanMessage can_message;
 
-	if (can_handler.socket < 0)
+	can_message = openxc_CanMessage_init_default;
+
+	/* read available data */
+	if ((revents & EPOLLIN) != 0)
 	{
-		return can_handler.socket;
+		read_can(&can_message);
+		send_event();
 	}
 
-	event_loop = afb_daemon_get_event_loop(interface->daemon);
-	rc = sd_event_add_io(event_loop, &source, can_handler.socket, EPOLLIN, on_event, NULL);
-	if (rc < 0)
-	{
-		close(can_handler.socket);
-		ERROR(interface, "Can't connect CAN bus %s to the event loop", can_handler.device);
-	} else
+	/* check if error or hangup */
+	if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0)
 	{
-		NOTICE(interface, "Connected CAN bus %s to the event loop", can_handler.device);
+		sd_event_source_unref(s);
+		close(fd);
+		connect_to_event_loop();
 	}
 
-	return rc;
+	return 0;
+}
+
+/*
+ * get or create an event handler for the type
+ */
+static event *get_event(uint32_t id, enum type type)
+{
+	event *event_elt;
+	can_event *list;
+
+	/* find the can list by id */
+	list = get_event_list_of_id(id);
+
+	/* make the new event */
+	event_elt = (event*)calloc(1, sizeof(event));
+	event_elt->next = event_elt;
+	list->events = event_elt;
+	event_elt->name = create_name(id);
+	event_elt->afb_event = afb_daemon_make_event(interface->daemon, event_elt->name);
+
+	return event_elt;
 }
 
 /*************************************************************************/
@@ -520,73 +546,89 @@ static int get_type_for_req(struct afb_req req, enum type *type)
 	return 0;
 }
 
-/*
- * subscribe to notification of new CAN messages
- *
- * parameters of the subscription are:
- *
- *    TODO type: string:  choose between CAN and OBDII messages formats.
- *
- * returns an object with 2 fields:
- *
- *    name:   string:  the name of the event without its prefix
- *    id:     integer: a numeric identifier of the event to be used for unsubscribing
- */
-static void subscribe(struct afb_req req)
+static int subscribe_unsubscribe_sig(struct afb_req request, int subscribe, struct signal *sig)
 {
-	enum type type;
-	const char *period;
-	event *event;
-	uint32_t id;
-	struct json_object *json;
-
-	if (get_type_for_req(req, &type))
-	{
-		id = (uint32_t)atoi(afb_req_value(req, "id"));
-		event = get_event(id, type);
-		if (event == NULL)
-			afb_req_fail(req, "out-of-memory", NULL);
-		else if (afb_req_subscribe(req, event->afb_event) != 0)
-			afb_req_fail_f(req, "failed", "afb_req_subscribe returned an error: %m");
-		else
-		{
-			/* TODO : build json openXC message to send. I guess */
-			json = json_object_new_object();
-			json_object_object_add(json, "name", json_object_new_string(event->name));
-			afb_req_success(req, json, NULL);
+	if (!afb_event_is_valid(sig->event)) {
+		if (!subscribe)
+			return 1;
+		sig->event = afb_daemon_make_event(afbitf->daemon, sig->name);
+		if (!afb_event_is_valid(sig->event)) {
+			return 0;
 		}
 	}
+
+	if (((subscribe ? afb_req_subscribe : afb_req_unsubscribe)(request, sig->event)) < 0) {
+		return 0;
+	}
+
+	return 1;
 }
 
-/*
- * unsubscribe a previous subscription
- *
- * parameters of the unsubscription are:
- *
- *    id:   integer: the numeric identifier of the event as returned when subscribing
- */
-static void unsubscribe(struct afb_req req)
+static int subscribe_unsubscribe_all(struct afb_req request, int subscribe)
 {
-	const char *id;
-	event *event;
+	int i, n, e;
 
-	id = afb_req_value(req, "id");
-	if (id == NULL)
-		afb_req_fail(req, "missing-id", NULL);
-	else
-	{
-		event = get_event_of_id(atoi(id));
-		if (event == NULL)
-			afb_req_fail(req, "bad-id", NULL);
-		else
-		{
-			afb_req_unsubscribe(req, event->afb_event);
-			afb_req_success(req, NULL, NULL);
+	n = sizeof OBD2_PIDS / sizeof * OBD2_PIDS;
+	e = 0;
+	for (i = 0 ; i < n ; i++)
+		e += !subscribe_unsubscribe_sig(request, subscribe, &OBD2_PIDS[i]);
+	return e == 0;
+}
+
+static int subscribe_unsubscribe_name(struct afb_req request, int subscribe, const char *name)
+{
+	struct signal *sig;
+
+	if (0 == strcmp(name, "*"))
+		return subscribe_unsubscribe_all(request, subscribe);
+
+	sig = getsig(name);
+	if (sig == NULL) {
+		return 0;
+	}
+
+	return subscribe_unsubscribe_sig(request, subscribe, sig);
+}
+
+static void subscribe_unsubscribe(struct afb_req request, int subscribe)
+{
+	int ok, i, n;
+	struct json_object *args, *a, *x;
+
+	/* makes the subscription/unsubscription */
+	args = afb_req_json(request);
+	if (args == NULL || !json_object_object_get_ex(args, "event", &a)) {
+		ok = subscribe_unsubscribe_all(request, subscribe);
+	} else if (json_object_get_type(a) != json_type_array) {
+		ok = subscribe_unsubscribe_name(request, subscribe, json_object_get_string(a));
+	} else {
+		n = json_object_array_length(a);
+		ok = 0;
+		for (i = 0 ; i < n ; i++) {
+			x = json_object_array_get_idx(a, i);
+			if (subscribe_unsubscribe_name(request, subscribe, json_object_get_string(x)))
+				ok++;
 		}
+		ok = (ok == n);
 	}
+
+	/* send the report */
+	if (ok)
+		afb_req_success(request, NULL, NULL);
+	else
+		afb_req_fail(request, "error", NULL);
+}
+
+static void subscribe(struct afb_req request)
+{
+	subscribe_unsubscribe(request, 1);
+}
+
+static void unsubscribe(struct afb_req request)
+{
+	subscribe_unsubscribe(request, 0);
 }
 
-// TODO: Have to change session management flag to AFB_SESSION_CHECK to use token auth
 static const struct afb_verb_desc_v1 verbs[]=
 {
   { .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,    .info= "subscribe to notification of CAN bus messages." },