--- /dev/null
+/***************************************************************************
+ * pdu.c
+ *
+ * Thu Nov 19 15:13:08 2009
+ * Copyright 2009 Marcin Miklas
+ * <marcin.miklas@teleca.com>
+ ****************************************************************************/
+#include <string.h>
+#include <netinet/in.h>
+#include <stdlib.h>
+#include "notaio.h"
+#include "pdu.h"
+
+ServiceMessage* pack_pdu(int sigid, uns8* payload, int payload_len, int* pdu_len) {
+/* Now we need to construct a Service Message to send to the Hello, world
+ * service at the other end. Our service message will look like this:
+ ARGUMENT ITEM
+0xA1 SIGID 0x41 LENGTH DATA
+Length of 8 bit Argument Length Data
+signal ID sigid type of data field
+
+ * Ok, so what exactly does all this mean? Let's go through it one
+ * field at a time:
+ * - 0xA1: this is the length of the following signal ID. 0xA1 means
+ * it will be 1 byte long (8 bits).
+ * - SIGID: Signal Identifiers are used by service nodes to recognize
+ * incoming signals (messages). A list of valid signals should be
+ * provided in the Service Interface Specification.
+ * - 0x41: Type of the following argument, or one of NULL, TRUE or
+ * FALSE for simple items where these values are enough.
+ * The bytes following the argument type field make up the value of
+ * the argument. For example, an argument type of 0x14 (unsigned 32-bit
+ * integer) is followed by 4 bytes of data, representing the integer
+ * value in question. In our case, the argument type is 0x41, which
+ * stands for binary data (in our case, the test string we will be
+ * sending). For binary data items such as strings, the argument item
+ * is further divided into two parts:
+ * - LENGTH: Length of the following binary data, in octets. This is an
+ * 8 bit value, so the maximum length is 255 octets. For longer items,
+ * use type 0x42, which has a 16-bit length field.
+ * - DATA: The amount of data specified by LENGTH.
+ *
+ * For more information about Service Messages, see Appendix D of the
+ * NoTA platform programming guide.
+ */
+ if (!payload) payload_len = 0;
+ ServiceMessage* smsg = (ServiceMessage*)malloc( SERVICE_MESSAGE_HEADER_LEN + payload_len );
+ smsg->sigid_length = 0xA1;
+ smsg->sigid[0] = sigid;
+ smsg->argtype = payload ? 0x42 : 0x01;
+ smsg->arglen = htons(payload_len);
+
+ if (payload) {
+ memcpy(smsg->argdata, payload, payload_len);
+ }
+
+ if (pdu_len) {
+ *pdu_len = SERVICE_MESSAGE_HEADER_LEN + payload_len;
+ if(!payload)
+ *pdu_len -= sizeof(smsg->arglen);
+ }
+
+
+ return smsg;
+}
+
+/* If we decide to discard a message before it is fully
+ * read, we'll need to
+ * have some way of abandoning remaining data on the
+ * connected socket.
+ * This function serves that purpose for the time being
+ * (that is, until HIN3 supports message-based sockets).
+ */
+static void clear(HSSockID socket,int quiet, ServiceCallbacks* cb)
+{
+ uns8 buf[64];
+ HErrorCode err;
+ int i=0;
+ do {
+ err = n_read(socket, &buf, 64, HSReceiveNonBlocking);
+ if (!quiet) {
+ while (i < 64) {
+ LOG1("%02x ", buf[i++]);
+ }
+ }
+ } while (err > 0);
+ if(!quiet)
+ LOG("\n");
+}
+
+HErrorCode read_smsg(HSSockID* socket, HSReceiveMode mode, ServiceCallbacks* cb)
+{
+ HErrorCode error;
+ ServiceMessage smsg;
+ int count = 0;
+
+ /* In the following lines, we will read the service message from
+ * the internal RX buffer into the 'smsg' variable. The first bytes
+ * of a NoTA service message will tell us what kind of message
+ * we're dealing with.
+ */
+ error = n_read(*socket, &smsg.sigid_length, sizeof(smsg.sigid_length), mode);
+ if (error <= 0)
+ return error;
+ count += error;
+ if (smsg.sigid_length != 0xA1)
+ {
+ //LOG("Only 8-bit signal identifiers currently recognized.\n");
+ clear(*socket, 1, cb);
+ return 0;
+ }
+ error = n_read(*socket, &smsg.sigid, 2, mode);
+ if (error <= 0)
+ return error;
+ count += error;
+ switch(smsg.sigid[0])
+ {
+ case PUT_IMAGE:
+ {
+ error = n_read(*socket, &smsg.argtype, 1, mode);
+ if (error <= 0)
+ return error;
+ if (smsg.argtype != 0x42)
+ {
+ clear(*socket, 0, cb);
+ return 0;
+ }
+ error = n_read(*socket, &smsg.arglen, 2, mode);
+ smsg.arglen = ntohs(smsg.arglen);
+ if (error <= 0)
+ return error;
+ uns8* argdata = malloc(smsg.arglen);
+ error = n_read(*socket, argdata, smsg.arglen, HSReceiveBlocking);
+ if (error == smsg.arglen)
+ {
+ if(cb->put_image)
+ cb->put_image(argdata,smsg.arglen);
+ }
+ else
+ {
+ LOG2("Error: got only %d bytes of %d expected.\n", error, smsg.arglen);
+ }
+
+ free((void*)argdata);
+ }
+ break;
+ case GET_IMAGE:
+ error = n_read(*socket, &smsg.argtype, 1, mode);
+ if (error <= 0)
+ return error;
+ if (smsg.argtype != 0x01)
+ {
+ clear(*socket, 0, cb);
+ return 0;
+ }
+ {
+ uns8* last_img=NULL;
+ int last_img_size=0;
+
+ ServiceMessage* smsg;
+ int smsg_len=0;
+
+ if (cb->get_image) {
+ last_img = cb->get_image(&last_img_size);
+ }
+
+ smsg = pack_pdu(PUT_IMAGE, last_img, last_img_size, &smsg_len);
+ error = n_send(*socket, smsg, smsg_len, HSSendBlocking);
+ free(smsg);
+
+ free(last_img);
+ }
+ break;
+ case FACE_FOUND:
+ {
+ error = n_read(*socket, &smsg.argtype, 1, mode);
+ if (error <= 0)
+ return error;
+ if (smsg.argtype != 0x42)
+ {
+ clear(*socket, 0, cb);
+ return 0;
+ }
+ error = n_read(*socket, &smsg.arglen, 2, mode);
+ smsg.arglen = ntohs(smsg.arglen);
+ if (error <= 0)
+ return error;
+
+ struct {int x,y,r;} face;
+ error = n_read(*socket, &face, sizeof(face), HSReceiveBlocking);
+ if (error == smsg.arglen) {
+ if(cb->face_found) {
+ cb->face_found(ntohl(face.x),ntohl(face.y),ntohl(face.r));
+ }
+ }
+ }
+ break;
+ case DISCONNECT:
+ if (cb->disconnect) {
+ cb->disconnect(*socket);
+ }
+ return DISCONNECT;
+ break;
+ case QUIT:
+ if (cb->quit) {
+ cb->quit(*socket);
+ }
+ return QUIT;
+ break;
+ default:
+ LOG("Unsupported service call.\n");
+ clear(*socket, 0, cb);
+ break;
+ }
+ return 0;
+}
projects / jamendo / blobdiff