179 lines
4.5 KiB
C

#include "lab.h"
void client_init(struct client *client, struct backend *backend)
{
memset(client, 0, sizeof(*client));
client->backend = backend;
for (size_t bank = 0; bank < BANKS; bank++) { client->value[bank][0] = 100; }
}
static int invoke(struct client *client, int operation, int a, int b,
unsigned char *bytes, size_t length)
{
int result;
do {
result = device_call(client->backend, operation, a, b, bytes, length);
} while (result == -EINTR);
return result;
}
static void status_encode(unsigned value, unsigned char *bytes, size_t width)
{
size_t index;
for (index = 0; index < width; index++) {
bytes[index] = (unsigned char)(value & 255u);
value >>= 8;
}
}
static int status_query(struct client *client, const struct command *command,
struct reply *reply)
{
const int *state = client->value[client->selected];
unsigned char bytes[32];
size_t index;
int start;
int count;
int width;
if (command->operation != 'Q' && command->operation != 'P') {
return 0;
}
if (!command_has(command, 2)) {
reply->result = -EINVAL;
return 1;
}
start = command->argument[0];
if (command->operation == 'Q') {
count = 1;
width = command->argument[1];
if (width != 1 && width != 2 && width != 4) {
reply->result = -EINVAL;
return 1;
}
} else {
count = command->argument[1];
width = 4;
}
if (start < 0 || count < 0 || start > 8 || count > 8 - start) {
reply->result = -EINVAL;
return 1;
}
memset(bytes, 0, sizeof(bytes));
for (index = 0; index < (size_t)count; index++) {
unsigned value = (unsigned)state[start + (int)index];
status_encode(value, bytes + index * (size_t)width, (size_t)width);
}
reply_copy(reply, bytes, (size_t)(count * width));
return 1;
}
static int command_kind(int magic, int number, int direction, int size)
{
/* COMMAND_WORD */
if (magic != 80) {
return -ENOTTY;
}
if (number == 1) {
if (direction != 2 || size != 8) {
return -ENOTTY;
}
return 1;
}
if (number == 2) {
if (direction != 1 || size != 8) {
return -ENOTTY;
}
return 2;
}
if (number == 3) {
if (direction != 0 || size != 0) {
return -ENOTTY;
}
return 3;
}
return -ENOTTY;
}
static int get_config(struct client *client, struct reply *reply)
{
int *state = client->value[client->selected];
unsigned char bytes[8];
int result;
memset(bytes, 0, sizeof(bytes));
result = invoke(client, 1, 0, 0, bytes, sizeof(bytes));
if (result) {
return result;
}
reply_copy(reply, bytes, sizeof(bytes));
state[2]++;
return 0;
}
static int set_config(struct client *client, int rate, int mode)
{
int *state = client->value[client->selected];
int result;
if (rate < 1 || rate > 1000 || mode < 0 || mode > 3) {
return -EINVAL;
}
/* TRANSACTION */
result = invoke(client, 2, rate, mode, NULL, 0);
if (result) {
return result;
}
state[0] = rate;
state[1] = mode;
state[3]++;
state[6]++;
return 0;
}
static int reset_config(struct client *client)
{
int *state = client->value[client->selected];
int result = invoke(client, 3, 0, 0, NULL, 0);
if (result) {
return result;
}
state[0] = 100;
state[1] = 0;
state[4]++;
state[6]++;
return 0;
}
static int ioctl_request(struct client *client, const struct command *command,
struct reply *reply)
{
int kind;
if (!command_has(command, 6)) {
return -EINVAL;
}
kind = command_kind(command->argument[0], command->argument[1],
command->argument[2], command->argument[3]);
if (kind < 0) {
return kind;
}
if (kind == 1) {
return get_config(client, reply);
}
if (kind == 2) {
return set_config(client, command->argument[4], command->argument[5]);
}
return reset_config(client);
}
void client_apply(struct client *client, const struct command *command,
struct reply *reply)
{
if (status_query(client, command, reply)) { return; }
if (command->operation != 'I') {
reply->result = -EINVAL;
return;
}
reply->result = ioctl_request(client, command, reply);
if (reply->result) {
client->value[client->selected][5]++;
}
}