devsus.c

T-kernel 的extension源代码

/*
 *----------------------------------------------------------------------
 *    T-Kernel / Standard Extension
 *
 *    Copyright (C) 2006 by Ken Sakamura. All rights reserved.
 *    T-Kernel / Standard Extension is distributed 
 *      under the T-License for T-Kernel / Standard Extension.
 *----------------------------------------------------------------------
 *
 *    Version:   1.00.00
 *    Released by T-Engine Forum(http://www.t-engine.org) at 2006/8/11.
 *
 *----------------------------------------------------------------------
 */

/*
 *	devsus.c (device)
 *
 *	Suspend the device.
 */

#include "devmgr.h"
#include <extension/message.h>
#include <extension/clk.h>


LOCAL ER SusResNotify( UW mode );
LOCAL WER Suspend( UW mode );

/*
 * Suspend/Resume notification message format
 */
typedef struct {
	W	type;	/* Message type (MS_SYS5) */
	W	size;	/* Message size */
	W	code;	/* Notification code (D_NOTSUS or D_NOTRES) */
} NotifyMsg;

/*
 * Suspend/Resume notification request management information
 */
typedef struct {
	W	pid;		/* Request process ID (0= Empty) */
	UW	mode;		/* Notification mode */
} NotifyEnt;

#define	N_NotifyEnt	32
LOCAL NotifyEnt		NotifyTbl[N_NotifyEnt];

#define TSD_SRM_VAL_0X12345678	(W)0x12345678
#define TSD_SPD_VAL_M1		(-1)
#define TSD_SDV_MSK_0X3000	0x3000U

/*
 * Set the suspend notification request.
 *	mode = D_NOTSUS || D_NOTRES || 0
 */
EXPORT ER SetSusResNotify( UW mode, W pid )
{
	NotifyEnt	*ent;
	W		i, n;
	ER		err;

	ent = NULL;
	for ( i = 0; i < N_NotifyEnt; ++i ) {
		n = NotifyTbl[i].pid;
		if ( (n == 0) || (n == pid) ) {
			ent = &NotifyTbl[i];
			if ( n == pid ) {
				break;
			}
		}
	}
	if ( ent == NULL ) {
		if ( mode != 0U ) {
			err = E_LIMIT;
			goto err_ret;
		}
		return E_OK; /* Nothing to delete. */
	}

	/* Registration/Deregistration */
	ent->pid  = ( mode == 0U )? 0: pid;
	ent->mode = mode;

	return E_OK;

err_ret:
	DEBUG_PRINT(("SetSusResNotify err = %d\n", err));
	return err;
}

/*
 * Suspend/Resume notification
 *	mode = D_NOTSUS || D_NOTRES
 */
LOCAL ER SusResNotify( UW mode )
{
const	W		TMO_RESP = 10 * 1000;	/* Timeout of the notification response */
const	W		TMO_CODE = TSD_SRM_VAL_0X12345678;	/* Timeout code */
	NotifyMsg	msg;
	W		i, n, m;
	W		pid, list[N_NotifyEnt];
	ER		err;

	/* Notification message */
	msg.type = MS_SYS5;
	msg.size = sizeof(W);
	msg.code = (W)mode;

	LockDM();
	n = 0;
	for ( i = 0; i < N_NotifyEnt; ++i ) {
		pid = NotifyTbl[i].pid;
		if ( (pid <= 0) || ((NotifyTbl[i].mode & mode) == 0U) ) {
			continue;
		}
		list[n++] = pid;
	}
	UnlockDM();

	for ( i = 0; i < n; ++i ) {
		/* Send the notification message. */
		err = tkse_snd_msg(list[i], (MESSAGE*)&msg, WAIT);
		if ( err < E_OK ) {
			continue;
		}
	}

	/* Wait the response only when it is a suspend notification. */
	if ( mode != D_NOTSUS ) {
		return E_OK;
	}

	/* Set the response waiting timeout. */
	tkse_req_tmg(TMO_RESP, TMO_CODE);

	m = 0;
	while ( m < n ) {
		/* Receive a response message. */
		pid = tkse_rcv_msg(MM_SYS5 | MM_TMOUT, (MESSAGE*)&msg,
						sizeof(msg), WAIT | CLR);
		if ( pid <= 0 ) {
			continue;
		}

		if ( msg.type == MS_SYS5 ) {
			/* Response message */
			for ( i = 0; i < n; ++i ) {
				if ( list[i] == pid ) {
					list[i] = 0;
					m++;
					break;
				}
			}
		} else {
			/* Timeout message */
			if ( msg.code == TMO_CODE ) {
				err = E_TMOUT;
				goto err_ret;
			}
		}
	}

	return E_OK;

err_ret:
	DEBUG_PRINT(("Notify err = %d\n", err));
	return err;
}

/* ------------------------------------------------------------------------ */

/*
 * Set the suspend.
 *	mode =	D_EMRGSUS
 *	     ||	D_SUSPEND | [D_FORCE]
 *	     ||	D_DISSUS		(Ignore D_FORCE)
 *	     ||	D_ENASUS		(Ignore D_FORCE)
 *	     ||	D_CHECK			(Ignore D_FORCE)
 *
 */
LOCAL WER Suspend( UW mode )
{
	UINT	m;
	W	suscnt = 0;
	ER	err, error = E_OK;

	switch ( mode & ~(UW)D_FORCE ) {
	  case D_EMRGSUS:
		m = TD_SUSPEND | TD_FORCE;
		break;
	  case D_SUSPEND:
		m = mode;
		break;
	  case D_DISSUS:
	  case D_ENASUS:
	  case D_CHECK:
		m = mode & ~(UW)D_FORCE;
		break;

	  default:
		error = E_PAR;
		goto err_ret;
	}

	mode &= ~(UW)D_FORCE;

	if ( mode == (UW)D_SUSPEND ) {
		/* Suspend notification */
		err = SusResNotify(D_NOTSUS);
		if ( err < E_OK ) {
			error = err;
		}
	}

	/* Suspend processing */
	err = tk_sus_dev(m);
	if ( err < E_OK ) {
		error = ERtoERR(err);
	}

	if ( mode == (UW)D_CHECK ) {
		suscnt = err;
	}

	if ( (mode == (UW)D_EMRGSUS) || (mode == (UW)D_SUSPEND) ) {

		/* Reset the clock. */
		err = tkse_set_tim2(TSD_SPD_VAL_M1, NULL);
		if ( err < E_OK ) {
			error = err;
		}

		/* Resume notification */
		err = SusResNotify(D_NOTRES);
		if ( err < E_OK ) {
			error = err;
		}
	}

	if ( error < E_OK ) {
		goto err_ret;
	}
	return suscnt;

err_ret:
	DEBUG_PRINT(("Suspend err = %d\n", error));
	return error;
}

/*
 * Suspend the device.
 *	mode =	D_EMRGSUS
 *	     ||	D_SUSPEND | [D_FORCE]
 *	     ||	D_DISSUS  | [D_FORCE]
 *	     ||	D_ENASUS  | [D_FORCE]
 *	     ||	D_CHECK   | [D_FORCE]
 *	     ||	D_NOTIFY  | [D_NOTSUS] | [D_NOTRES]
 */
EXPORT WER _tkse_sus_dev( UW mode )
{
	W	pid;
	W	suscnt = 0;
	ER	err;

	/* Call from other than process is prohibited. */
	pid = GetMyPid();
	if ( pid < E_OK ) {
		err = pid;
		goto err_ret;
	}

	switch ( mode & TSD_SDV_MSK_0X3000 ) {
	  case 0:
		suscnt = Suspend(mode);

		if ( suscnt < E_OK ) {
			err = suscnt;
			goto err_ret;
		}
		break;

	  case D_NOTIFY:
		err = SetSusResNotify(mode & ~TSD_SDV_MSK_0X3000, pid);
		if ( err < E_OK ) {
			goto err_ret;
		}
		break;

	  default:
		err = E_PAR;
		goto err_ret;
	}

	return suscnt;

err_ret:
	DEBUG_PRINT(("_tkse_sus_dev err = %d\n", err));
	return err;
}