/*----------------------------------------------------------------------------*\
   Read method for the Modbus RTU server module
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardRead (
  io_tCtx	ctx,
  io_sAgent	*ap,
  io_sRack	*rp,
  io_sCard	*cp
) 
{
  io_sServerModuleLocal *local;
  io_sServerLocal *local_server;
  pwr_sClass_Modbus_RTU_ServerModule *op;
  pwr_sClass_Modbus_RTU_Server *server;

  op = (pwr_sClass_Modbus_RTU_ServerModule *) cp->op;
  local = (io_sServerModuleLocal *) cp->Local;
  server = (pwr_sClass_Modbus_RTU_Server *) rp->op;
  local_server = (io_sServerLocal *) rp->Local;

  if ( server->DisableServer || !local)
    return IO__SUCCESS;

  if (server->Status == MB__NORMAL) { 
    thread_MutexLock( &local_server->mutex);

    io_bus_card_read(ctx, rp, cp, local->input_area, NULL,  pwr_eByteOrderingEnum_BigEndian, pwr_eFloatRepEnum_FloatIntel);  

    thread_MutexUnlock( &local_server->mutex);
  }
//  printf("Method Modbus_Module-IoCardRead\n");
  return IO__SUCCESS;
}
Пример #2
0
void
que_Put (
  pwr_tStatus *status,
  que_sQue *qp,
  lst_sEntry *lp,
  void *item
)
{
  pwr_dStatus(sts, status, QUE__SUCCESS);

  thread_MutexLock(&qp->mutex);

    lst_InsertPred(NULL, &qp->lh, lp, item);
    thread_CondSignal(&qp->cond);

  thread_MutexUnlock(&qp->mutex);
}
Пример #3
0
void *
que_Get (
  pwr_tStatus *status,
  que_sQue *qp,
  pwr_tDeltaTime *tp,
  void *tmo_item
)
{
  void *p = NULL;
  pwr_dStatus(sts, status, QUE__SUCCESS);

  thread_MutexLock(&qp->mutex);

    while((p = lst_RemoveSucc(NULL, &qp->lh, NULL)) == NULL && ODD(*sts) && *sts != THREAD__TIMEDOUT)
      *sts = thread_CondTimedWait(&qp->cond, &qp->mutex, tp);

  thread_MutexUnlock(&qp->mutex);

  return p == NULL ? tmo_item : p;
}
/* C_DigestKey continues a multi-part message-digesting
 * operation, by digesting the value of a secret key as part of
 * the data already digested. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestKey)
(
  CK_SESSION_HANDLE hSession,  /* the session's handle */
  CK_OBJECT_HANDLE  hKey       /* secret key to digest */
)
{
    CK_RV rv = CKR_OK;

    P11_LOG_START("C_DigestKey");

    thread_MutexLock(st.async_lock);

    rv = CKR_FUNCTION_NOT_SUPPORTED;
    log_Log(LOG_MED, "Function not supported");

    thread_MutexUnlock(st.async_lock);

    P11_LOG_END("C_DigestKey");

    return rv;
}
/* C_DigestInit initializes a message-digesting operation. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestInit)
(
  CK_SESSION_HANDLE hSession,   /* the session's handle */
  CK_MECHANISM_PTR  pMechanism  /* the digesting mechanism */
)
{
    CK_RV rv = CKR_OK;

    P11_LOG_START("C_DigestInit");

    thread_MutexLock(st.async_lock);

    rv = CKR_FUNCTION_NOT_SUPPORTED;
    log_Log(LOG_MED, "Function not supported");

    thread_MutexUnlock(st.async_lock);

    P11_LOG_END("C_DigestInit");

    return rv;
}
/* C_DigestUpdate continues a multiple-part message-digesting
 * operation. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestUpdate)
(
  CK_SESSION_HANDLE hSession,  /* the session's handle */
  CK_BYTE_PTR       pPart,     /* data to be digested */
  CK_ULONG          ulPartLen  /* bytes of data to be digested */
)
{
    CK_RV rv = CKR_OK;

    P11_LOG_START("C_DigestUpdate");

    thread_MutexLock(st.async_lock);

    rv = CKR_FUNCTION_NOT_SUPPORTED;
    log_Log(LOG_MED, "Function not supported");

    thread_MutexUnlock(st.async_lock);

    P11_LOG_END("C_DigestUpdate");

    return rv;
}
/* C_DigestFinal finishes a multiple-part message-digesting
 * operation. */
CK_DEFINE_FUNCTION(CK_RV, C_DigestFinal)
(
  CK_SESSION_HANDLE hSession,     /* the session's handle */
  CK_BYTE_PTR       pDigest,      /* gets the message digest */
  CK_ULONG_PTR      pulDigestLen  /* gets byte count of digest */
)
{
    CK_RV rv = CKR_OK;

    P11_LOG_START("C_DigestFinal");

    thread_MutexLock(st.async_lock);

    rv = CKR_FUNCTION_NOT_SUPPORTED;
    log_Log(LOG_MED, "Function not supported");

    thread_MutexUnlock(st.async_lock);

    P11_LOG_END("C_DigestFinal");

    return rv;
}
Пример #8
0
static void *mb_receive( void *data)
{
  io_sRack *rp = ((mb_sCondata *)data)->rp;
  int l_idx = ((mb_sCondata *)data)->idx;
  io_sServerLocal* local = rp->Local;
  int c_socket = local->connections[l_idx].c_socket;
  pwr_sClass_Modbus_TCP_Server *op = (pwr_sClass_Modbus_TCP_Server *) rp->op;
  ssize_t data_size;
  rec_buf *rb;
  unsigned char fc;
  unsigned char exception_code;
  ssize_t ssts;
  int size_of_msg;

  free( data);
  op->Connections++;

  while ( 1) {
    size_of_msg = 0;

    data_size = recv(c_socket, rcv_buffer, sizeof(rec_buf), 0);
    if ( data_size < 0) {
      op->ErrorCount++;
      continue;
    }
    if ( data_size == 0) {
      /* Disconnected */
      op->Connections--;
      close( c_socket);
      local->connections[l_idx].occupied = 0;
      errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
      return 0;
    }
    if ( op->DisableServer)
      continue;

    while ( data_size > 0) {

      if (data_size < sizeof(mbap_header))
	break;

      op->RX_packets++;

      rb = (rec_buf *) &rcv_buffer[size_of_msg];

      if ( rb->head.length == 0)
	break;

      size_of_msg += ntohs(rb->head.length) + 6;
      data_size -= ntohs(rb->head.length) + 6;

      fc = (unsigned char) *rb->buf;

      time_GetTime( &local->connections[l_idx].last_req_time);
      exception_code = 0;

      switch ( fc) {
      case pwr_eModbus_FCEnum_ReadHoldingRegisters: {
	io_sCard *cardp;
	io_sServerModuleLocal *local_card;
	pwr_sClass_Modbus_TCP_ServerModule *mp;
	read_req *rmsg = (read_req *)rb;
	rsp_read msg;
	int found;

	short addr = ntohs( rmsg->addr);
	short quant = ntohs( rmsg->quant);
	unsigned char unit_id = rmsg->head.unit_id;

	if ( quant < 1 || quant >= 0x07d0) {
	  exception_code = 3;
	  break;
	}

	/* Check the address */
	found = 0;
	for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
	  mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
	  if ( mp->UnitId == unit_id) {
	    local_card = cardp->Local;
	    found = 1;
	    break;
	  }	
	}

	if ( !found) {
	  exception_code = 2;
	  break;
	}

	addr -= mp->ReadAddress;

	if ( addr < 0 || 
	     addr + quant * 2 > local_card->output_size) {
	  exception_code = 2;
	  break;
	}

	msg.fc = fc;
	msg.bc = quant * 2;
	msg.head.trans_id = rmsg->head.trans_id;
	// msg.head.length = htons( sizeof(msg) - 6);
	msg.head.length = htons( sizeof(msg) - sizeof(msg.buf) + quant * 2 - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;

	thread_MutexLock( &local->mutex);
	memcpy( msg.buf, (char *)local_card->output_area + addr, quant * 2);
	thread_MutexUnlock( &local->mutex);

	ssts = send( c_socket, &msg, ntohs(msg.head.length) + 6, MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      case pwr_eModbus_FCEnum_ReadCoils:
      case pwr_eModbus_FCEnum_ReadDiscreteInputs: {
	io_sCard *cardp;
	io_sServerModuleLocal *local_card;
	pwr_sClass_Modbus_TCP_ServerModule *mp;
	read_req *rmsg = (read_req *)rb;
	rsp_read msg;
	int found;
	unsigned char mask;
	unsigned int bytes;
	int i;
	int offs;

	short addr = ntohs( rmsg->addr);
	short quant = ntohs( rmsg->quant);
	unsigned char unit_id = rmsg->head.unit_id;

	if ( quant < 1 || quant >= 0x07d0) {
	  exception_code = 3;
	  break;
	}

	/* Check the address */
	found = 0;
	for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
	  mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
	  if ( mp->UnitId == unit_id) {
	    local_card = cardp->Local;
	    found = 1;
	    break;
	  }	
	}

	if ( !found) {
	  exception_code = 2;
	  break;
	}

	offs = addr / 8;
	bytes = (addr + quant) / 8 +  (((addr + quant) % 8 == 0) ? 0 : 1) - offs;

	if ( addr < 0 || 
	     offs + bytes + local_card->do_offset > local_card->output_size || 
	     offs + bytes > local_card->do_size) {
	  exception_code = 2;
	  break;
	}

	memset( &msg, 0, sizeof(msg));
	msg.fc = fc;
	msg.bc = bytes;
	msg.head.trans_id = rmsg->head.trans_id;
	// msg.head.length = htons( sizeof(msg) - 6);
	msg.head.length = htons( sizeof(msg) - sizeof(msg.buf) + bytes - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;

	thread_MutexLock( &local->mutex);
	if ( addr % 8 == 0) {
	  memcpy( msg.buf, (char *)local_card->output_area + local_card->do_offset + addr/8, bytes);      

	  mask = 0;
	  for ( i = 0; i < quant % 8; i++)
	    mask |= 1 << i;

	  if ( quant % 8 != 0) {
	    unsigned char *b = (unsigned char *) msg.buf;
	    b[bytes - 1] &= mask;
	  }
	}
	else {
	  mb_shift_read( (unsigned char *)local_card->output_area + local_card->do_offset + addr / 8,
			 (unsigned char *)msg.buf,
			 addr % 8, quant);
	}
	thread_MutexUnlock( &local->mutex);

	ssts = send( c_socket, &msg, ntohs(msg.head.length) + 6, MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      case pwr_eModbus_FCEnum_WriteSingleRegister: {
	io_sCard *cardp;
	io_sServerModuleLocal *local_card;
	pwr_sClass_Modbus_TCP_ServerModule *mp;
	write_single_req *rmsg = (write_single_req *)rb;
	rsp_single_write msg;
	int found;

	short addr = ntohs( rmsg->addr);
	unsigned char unit_id = rmsg->head.unit_id;

	/* Check the address */
	found = 0;
	for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
	  mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
	  if ( mp->UnitId == unit_id) {
	    local_card = cardp->Local;
	    found = 1;
	    break;
	  }	
	}

	if ( !found) {
	  exception_code = 2;
	  break;
	}

	addr -= mp->WriteAddress;

	if ( addr < 0 || 
	     addr + 2 > local_card->input_size) {
	  exception_code = 2;
	  break;
	}

	thread_MutexLock( &local->mutex);
	memcpy( (char *)local_card->input_area + addr, &rmsg->value, 2);
	thread_MutexUnlock( &local->mutex);

	msg.fc = fc;
	msg.addr = rmsg->addr;
	msg.value = rmsg->value;
	msg.head.trans_id = rmsg->head.trans_id;
	msg.head.length = htons( sizeof(msg) - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;

	ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      case pwr_eModbus_FCEnum_WriteMultipleRegisters: {
	io_sCard *cardp;
	io_sServerModuleLocal *local_card;
	pwr_sClass_Modbus_TCP_ServerModule *mp;
	write_reg_req *rmsg = (write_reg_req *)rb;
	rsp_write msg;
	int found;

	short addr = ntohs( rmsg->addr);
	short quant = ntohs( rmsg->quant);
	unsigned char unit_id = rmsg->head.unit_id;

	if ( quant < 1 || quant >= 0x07d0) {
	  exception_code = 3;
	  break;
	}

	/* Check the address */
	found = 0;
	for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
	  mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
	  if ( mp->UnitId == unit_id) {
	    local_card = cardp->Local;
	    found = 1;
	    break;
	  }	
	}

	if ( !found) {
	  exception_code = 2;
	  break;
	}

	addr -= mp->WriteAddress;

	if ( addr < 0 || 
	     addr + quant * 2 > local_card->input_size) {
	  exception_code = 2;
	  break;
	}

	thread_MutexLock( &local->mutex);
	memcpy( (char *)local_card->input_area + addr, rmsg->reg, quant * 2);
	thread_MutexUnlock( &local->mutex);

	msg.fc = fc;
	msg.addr = rmsg->addr;
	msg.quant = rmsg->quant;
	msg.head.trans_id = rmsg->head.trans_id;
	msg.head.length = htons( sizeof(msg) - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;

	ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      case pwr_eModbus_FCEnum_WriteSingleCoil: {
	io_sCard *cardp;
	io_sServerModuleLocal *local_card;
	pwr_sClass_Modbus_TCP_ServerModule *mp;
	write_single_req *rmsg = (write_single_req *)rb;
	rsp_single_write msg;
	int found;
	unsigned char mask;
	int offs;

	short addr = ntohs( rmsg->addr);
	unsigned short value = ntohs( rmsg->value);
	unsigned char unit_id = rmsg->head.unit_id;

	/* Check the address */
	found = 0;
	for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
	  mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
	  if ( mp->UnitId == unit_id) {
	    local_card = cardp->Local;
	    found = 1;
	    break;
	  }	
	}

	if ( !found) {
	  exception_code = 2;
	  break;
	}

	offs = addr / 8;

	if ( addr < 0 || 
	     offs + local_card->di_offset >= local_card->input_size || 
	     offs >= local_card->di_size) {
	  exception_code = 2;
	  break;
	}

	mask = 1 << (addr % 8);
	if ( value == 0xFF00 || value == 0) {
	  thread_MutexLock( &local->mutex);
	  if ( value == 0xFF00)
	    *((char *)local_card->input_area + local_card->di_offset + offs) |= mask;
	  else
	    *((char *)local_card->input_area + local_card->di_offset + offs) &= ~mask;
	  thread_MutexUnlock( &local->mutex);
	}
	msg.fc = fc;
	msg.addr = rmsg->addr;
	msg.value = rmsg->value;
	msg.head.trans_id = rmsg->head.trans_id;
	msg.head.length = htons( sizeof(msg) - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;

	ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      case pwr_eModbus_FCEnum_WriteMultipleCoils: {
	io_sCard *cardp;
	io_sServerModuleLocal *local_card;
	pwr_sClass_Modbus_TCP_ServerModule *mp;
	write_reg_req *rmsg = (write_reg_req *)rb;
	rsp_write msg;
	int found;
	unsigned char mask;
	unsigned int bytes;
	int i;
	int offs;

	short addr = ntohs( rmsg->addr);
	short quant = ntohs( rmsg->quant);
	unsigned char unit_id = rmsg->head.unit_id;

	if ( quant < 1 || quant >= 0x07d0) {
	  exception_code = 3;
	  break;
	}

	/* Check the address */
	found = 0;
	for ( cardp = rp->cardlist; cardp; cardp = cardp->next) {
	  mp = (pwr_sClass_Modbus_TCP_ServerModule *) cardp->op;
	  if ( mp->UnitId == unit_id) {
	    local_card = cardp->Local;
	    found = 1;
	    break;
	  }	
	}

	if ( !found) {
	  exception_code = 2;
	  break;
	}

	offs = addr / 8;
	bytes = (addr + quant) / 8 +  (((addr + quant) % 8 == 0) ? 0 : 1) - offs;

	if ( addr < 0 || 
	     offs + bytes + local_card->di_offset > local_card->input_size || 
	     offs + bytes > local_card->di_size) {
	  exception_code = 2;
	  break;
	}

	thread_MutexLock( &local->mutex);
	if ( addr % 8 == 0) {
	  if ( quant % 8 != 0) {
	    mask = 0;
	    for ( i = 0; i < quant % 8; i++)
	      mask |= 1 << i;
	  
	    memcpy( (char *)local_card->input_area + local_card->di_offset + addr / 8, 
		    rmsg->reg, bytes - 1);
	    *((char *)local_card->input_area + local_card->di_offset + addr / 8 + bytes - 1) &= ~mask;
	    *((char *)local_card->input_area + local_card->di_offset + addr / 8 + bytes - 1) |= *((char *)rmsg->reg + bytes - 1) & mask;
	  }
	  else
	    memcpy( (char *)local_card->input_area + local_card->di_offset + addr / 8, 
		    rmsg->reg, bytes);
	}
	else {
	  mb_shift_write( (unsigned char *)rmsg->reg, 
			  (unsigned char *)local_card->input_area + local_card->di_offset + addr / 8,
			  addr % 8, quant);
	}
	thread_MutexUnlock( &local->mutex);
	
	msg.fc = fc;
	msg.addr = rmsg->addr;
	msg.quant = rmsg->quant;
	msg.head.trans_id = rmsg->head.trans_id;
	msg.head.length = htons( sizeof(msg) - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;

	ssts = send( c_socket, &msg, sizeof(msg), MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      case 43: {
	/* Encapsulated Interface Transport, Read Device Identification */
	read_dev_id_req *rmsg = (read_dev_id_req *)rb;
	rsp_dev_id msg;
	int i;
	int len;

	if ( rmsg->mei_type != 0x2b) {
	  exception_code = 1;
	  break;
	}

	if ( rmsg->id_code != 1) {
	  exception_code = 1;
	  break;
	}

	if ( rmsg->object_id != 0) {
	  exception_code = 1;
	  break;
	}

	msg.fc = rmsg->fc;
	msg.mei_type = rmsg->mei_type;
	msg.id_code = rmsg->id_code;
	msg.conformity_level = 1;
	msg.more_follows = 0;
	msg.next_object_id = 0;
	msg.number_of_objects = 3;

	i = 0;

	/* Vendor name */
	msg.list[i++] = 0;
	len = strlen("Proview");
	msg.list[i++] = len;
	strncpy( (char *)&msg.list[i], "Proview", len);
	i += len;

	/* Product code */
	msg.list[i++] = 0;
	len = strlen("-");
	msg.list[i++] = len;
	strncpy( (char *)&msg.list[i], "-", len);
	i += len;

	/* Major Minor Revision */
	msg.list[i++] = 0;
	len = strlen(pwrv_cPwrVersionStr);
	msg.list[i++] = len;
	strncpy( (char *)&msg.list[i], pwrv_cPwrVersionStr, len);
	i += len;

	msg.head.trans_id = rmsg->head.trans_id;
	msg.head.length = htons( sizeof(msg) - sizeof(msg.list) + i - 6);
	msg.head.unit_id = rmsg->head.unit_id;
	msg.head.proto_id = rmsg->head.proto_id;
	
	ssts = send( c_socket, &msg, ntohs(msg.head.length) + 6, MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;      
      
	break;
      }
      default:
	exception_code = 1;
      }

      if ( exception_code) {
	rsp_fault rsp_f;

	rsp_f.fc = fc + 0x80;
	rsp_f.ec = exception_code;
	rsp_f.head.trans_id = rb->head.trans_id;
	rsp_f.head.length = htons( sizeof(rsp_f) - 6);
	rsp_f.head.unit_id = rb->head.unit_id;

	ssts = send( c_socket, &rsp_f, sizeof(rsp_f), MSG_DONTWAIT);
	if (ssts < 0) {
	  op->Connections--;
	  close(c_socket);
	  local->connections[l_idx].occupied = 0;
	  errh_Error( "Connection lost for IO modbus tcp server %s, %d", rp->Name, c_socket);
	  return 0;
	}
	op->TX_packets++;
      }
    }    
  }
  return 0;
}