+++ /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;
-}