recipes-devtools/switch/files/src/switch_netlink.c

/*
 * switch_netlink.c: switch(netlink) set API
 *
 * Author: Sirui Zhao <Sirui.Zhao@mediatek.com>
 */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netlink/netlink.h>
#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>

#include "switch_netlink.h"

static struct nl_sock *user_sock;
static struct nl_cache *cache;
static struct genl_family *family;
static struct nlattr *attrs[MT753X_ATTR_TYPE_MAX + 1];

static int wait_handler(struct nl_msg *msg, void *arg)
{
	int *finished = arg;

	*finished = 1;
	return NL_STOP;
}

static int list_swdevs(struct nl_msg *msg, void *arg)
{
	struct mt753x_attr *val = arg;
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));

	if (nla_parse(attrs, MT753X_ATTR_TYPE_MAX, genlmsg_attrdata(gnlh, 0),
		      genlmsg_attrlen(gnlh, 0), NULL) < 0)
		goto done;

	if (gnlh->cmd == MT753X_CMD_REPLY) {
		if (attrs[MT753X_ATTR_TYPE_MESG]) {
			val->dev_info =
			    nla_get_string(attrs[MT753X_ATTR_TYPE_MESG]);
			printf("register switch dev:\n%s", val->dev_info);
		} else {
			printf("ERROR:No switch dev now\n");
			goto done;
		}
	} else
		goto done;
	return 0;
done:
	return NL_SKIP;
}

static int construct_attrs(struct nl_msg *msg, void *arg)
{
	struct mt753x_attr *val = arg;
	int type = val->type;

	if (val->dev_id > -1)
		NLA_PUT_U32(msg, MT753X_ATTR_TYPE_DEV_ID, val->dev_id);

	if (val->op == 'r') {
		if (val->phy_dev != -1)
			NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY_DEV,
				    val->phy_dev);
		if (val->port_num >= 0)
			NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY, val->port_num);
		NLA_PUT_U32(msg, type, val->reg);
	} else if (val->op == 'w') {
		if (val->phy_dev != -1)
			NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY_DEV,
				    val->phy_dev);
		if (val->port_num >= 0)
			NLA_PUT_U32(msg, MT753X_ATTR_TYPE_PHY, val->port_num);
		NLA_PUT_U32(msg, type, val->reg);
		NLA_PUT_U32(msg, MT753X_ATTR_TYPE_VAL, val->value);
	} else {
		printf("construct_attrs_message\n");
		NLA_PUT_STRING(msg, type, "hello");
	}
	return 0;

nla_put_failure:
	return -1;
}

static int spilt_attrs(struct nl_msg *msg, void *arg)
{
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct mt753x_attr *val = arg;
	char *str;

	if (nla_parse(attrs, MT753X_ATTR_TYPE_MAX, genlmsg_attrdata(gnlh, 0),
		      genlmsg_attrlen(gnlh, 0), NULL) < 0)
		goto done;

	if ((gnlh->cmd == MT753X_CMD_WRITE) || (gnlh->cmd == MT753X_CMD_READ)) {
		if (attrs[MT753X_ATTR_TYPE_MESG]) {
			str = nla_get_string(attrs[MT753X_ATTR_TYPE_MESG]);
			printf(" %s\n", str);
			if (!strncmp(str, "No", 2))
				goto done;
		}
		if (attrs[MT753X_ATTR_TYPE_REG]) {
			val->reg = nla_get_u32(attrs[MT753X_ATTR_TYPE_REG]);
		}
		if (attrs[MT753X_ATTR_TYPE_VAL]) {
			val->value = nla_get_u32(attrs[MT753X_ATTR_TYPE_VAL]);
		}
	} else
		goto done;

	return 0;
done:
	return NL_SKIP;
}

static int mt753x_request_callback(int cmd,
				   int (*spilt)(struct nl_msg *, void *),
				   int (*construct)(struct nl_msg *, void *),
				   void *arg)
{
	struct nl_msg *msg;
	struct nl_cb *callback = NULL;
	int finished;
	int flags = 0;
	int err;

	/* Allocate an netllink message buffer */
	msg = nlmsg_alloc();
	if (!msg) {
		printf("Failed to allocate netlink message\n");
		exit(1);
	}
	if (!construct) {
		if (cmd == MT753X_CMD_REQUEST)
			flags |= NLM_F_REQUEST;
		else
			flags |= NLM_F_DUMP;
	}
	genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, genl_family_get_id(family),
		    0, flags, cmd, 0);

	/* Fill attaribute of netlink message by construct function */
	if (construct) {
		err = construct(msg, arg);
		if (err < 0) {
			printf("attributes error\n");
			goto nal_put_failure;
		}
	}

	/* Allocate an new callback handler */
	callback = nl_cb_alloc(NL_CB_CUSTOM);
	if (!callback) {
		printf("Failed to allocate callback handler\n");
		exit(1);
	}

	/* Send netlink message */
	err = nl_send_auto_complete(user_sock, msg);
	if (err < 0) {
		printf("nl_send_auto_complete failied:%d\n", err);
		goto out;
	}
	finished = 0;
	if (spilt)
		nl_cb_set(callback, NL_CB_VALID, NL_CB_CUSTOM, spilt, arg);

	if (construct)
		nl_cb_set(callback, NL_CB_ACK, NL_CB_CUSTOM, wait_handler,
			  &finished);
	else
		nl_cb_set(callback, NL_CB_FINISH, NL_CB_CUSTOM, wait_handler,
			  &finished);

	/* Receive message from kernel request */
	err = nl_recvmsgs(user_sock, callback);
	if (err < 0)
		goto out;

	/* Wait until an ACK is received for the latest not yet acknowledge */
	if (!finished)
		err = nl_wait_for_ack(user_sock);
out:
	if (callback)
		nl_cb_put(callback);

nal_put_failure:
	nlmsg_free(msg);
	return err;
}

void mt753x_netlink_free(void)
{
	if (family)
		nl_object_put((struct nl_object *)family);
	if (cache)
		nl_cache_free(cache);
	if (user_sock)
		nl_socket_free(user_sock);
	user_sock = NULL;
	cache = NULL;
	family = NULL;
}

int mt753x_netlink_init(const char *name)
{
	int ret;

	user_sock = NULL;
	cache = NULL;
	family = NULL;

	/* Allocate an new netlink socket */
	user_sock = nl_socket_alloc();
	if (!user_sock) {
		printf("Failed to create user socket\n");
		goto err;
	}
	/* Connetct the genl controller */
	if (genl_connect(user_sock)) {
		printf("Failed to connetct to generic netlink\n");
		goto err;
	}
	/* Allocate an new nl_cache */
	ret = genl_ctrl_alloc_cache(user_sock, &cache);
	if (ret < 0) {
		printf("Failed to allocate netlink cache\n");
		goto err;
	}

	if (name == NULL)
		return -EINVAL;

	/* Look up generic netlik family by "mt753x" in the provided cache */
	family = genl_ctrl_search_by_name(cache, name);
	if (!family) {
		/* printf("switch(mt753x) API not be prepared\n"); */
		goto err;
	}
	return 0;
err:
	mt753x_netlink_free();
	return -EINVAL;
}

void mt753x_list_swdev(struct mt753x_attr *arg, int cmd)
{
	int err;

	err = mt753x_request_callback(cmd, list_swdevs, NULL, arg);
	if (err < 0)
		printf("mt753x list dev error\n");
}

static int mt753x_request(struct mt753x_attr *arg, int cmd)
{
	int err;

	err = mt753x_request_callback(cmd, spilt_attrs, construct_attrs, arg);
	if (err < 0) {
		printf("mt753x deal request error\n");
		return err;
	}
	return 0;
}

static int phy_operate_netlink(char op, struct mt753x_attr *arg,
			       unsigned int port_num, unsigned int phy_dev,
			       unsigned int offset, unsigned int *value)
{
	int ret = 0;
	struct mt753x_attr *attr = arg;

	attr->port_num = port_num;
	attr->phy_dev = phy_dev;
	attr->reg = offset;
	attr->value = 0;
	attr->type = MT753X_ATTR_TYPE_REG;

	switch (op) {
	case 'r':
		attr->op = 'r';
		ret = mt753x_request(attr, MT753X_CMD_READ);
		*value = attr->value;
		break;
	case 'w':
		attr->op = 'w';
		attr->value = *value;
		ret = mt753x_request(attr, MT753X_CMD_WRITE);
		break;
	default:
		break;
	}

	return ret;
}

int reg_read_netlink(struct mt753x_attr *arg, unsigned int offset,
		     unsigned int *value)
{
	int ret;

	ret = phy_operate_netlink('r', arg, -1, -1, offset, value);
	return ret;
}

int reg_write_netlink(struct mt753x_attr *arg, unsigned int offset,
		      unsigned int value)
{
	int ret;

	ret = phy_operate_netlink('w', arg, -1, -1, offset, &value);
	return ret;
}

int phy_cl22_read_netlink(struct mt753x_attr *arg, unsigned int port_num,
			  unsigned int phy_addr, unsigned int *value)
{
	int ret;

	ret = phy_operate_netlink('r', arg, port_num, -1, phy_addr, value);
	return ret;
}

int phy_cl22_write_netlink(struct mt753x_attr *arg, unsigned int port_num,
			   unsigned int phy_addr, unsigned int value)
{
	int ret;

	ret = phy_operate_netlink('w', arg, port_num, -1, phy_addr, &value);
	return ret;
}

int phy_cl45_read_netlink(struct mt753x_attr *arg, unsigned int port_num,
			  unsigned int phy_dev, unsigned int phy_addr,
			  unsigned int *value)
{
	int ret;

	ret = phy_operate_netlink('r', arg, port_num, phy_dev, phy_addr, value);
	return ret;
}

int phy_cl45_write_netlink(struct mt753x_attr *arg, unsigned int port_num,
			   unsigned int phy_dev, unsigned int phy_addr,
			   unsigned int value)
{
	int ret;

	ret =
	    phy_operate_netlink('w', arg, port_num, phy_dev, phy_addr, &value);
	return ret;
}

void dump_extend_phy_reg(struct mt753x_attr *arg, int port_no, int from,
			 int to, int is_local, int page_no)
{
	unsigned int temp = 0;
	int r31 = 0;
	int i = 0;

	if (is_local == 0) {
		printf("\n\nGlobal Register Page %d\n", page_no);
		printf("===============");
		r31 |= 0 << 15;	/* global */
		r31 |= ((page_no & 0x7) << 12);	/* page no */
		phy_cl22_write_netlink(arg, port_no, 31, r31);	/* select global page x */
		for (i = 16; i < 32; i++) {
			if (i % 8 == 0)
				printf("\n");
			phy_cl22_read_netlink(arg, port_no, i, &temp);
			printf("%02d: %04X ", i, temp);
		}
	} else {
		printf("\n\nLocal Register Port %d Page %d\n", port_no,
		       page_no);
		printf("===============");
		r31 |= 1 << 15;	/* Local */
		r31 |= ((page_no & 0x7) << 12);	/* Page no */
		phy_cl22_write_netlink(arg, port_no, 31, r31);	/* Select global page x */
		for (i = 16; i < 32; i++) {
			if (i % 8 == 0)
				printf("\n");
			phy_cl22_read_netlink(arg, port_no, i, &temp);
			printf("%02d: %04X ", i, temp);
		}
	}
	printf("\n");
}

int phy_dump_netlink(struct mt753x_attr *arg, int phy_addr)
{
	int i;
	int ret;
	unsigned int offset, value, phy_base = 0;

	if (chip_name == 0x8855)
		phy_base = 6;

	if (phy_addr == 32) {
		/* Dump all phy register */
		for (i = 0; i < 5; i++) {
			printf("\n[Port %d]=============", i);
			for (offset = 0; offset < 16; offset++) {
				if (offset % 8 == 0)
					printf("\n");
				ret =
				    phy_cl22_read_netlink(arg, phy_base + i,
							  offset, &value);
				printf("%02d: %04X ", offset, value);
			}
		}
	} else {
		printf("\n[Port %d]=============", phy_addr);
		for (offset = 0; offset < 16; offset++) {
			if (offset % 8 == 0)
				printf("\n");
			ret =
			    phy_cl22_read_netlink(arg, phy_addr, offset,
						  &value);
			printf("%02d: %04X ", offset, value);
		}
	}
	printf("\n");
	for (offset = 0; offset < 5; offset++) {	/* Global register  page 0~4 */
		if (phy_addr == 32)	/* Dump all phy register */
			dump_extend_phy_reg(arg, 0, 16, 31, 0, offset);
		else
			dump_extend_phy_reg(arg, phy_addr, 16, 31, 0, offset);
	}

	if (phy_addr == 32) {	/* Dump all phy register */
		for (offset = 0; offset < 5; offset++) {	/* Local register port 0-port4 */
			dump_extend_phy_reg(arg, phy_base + offset, 16, 31, 1, 0);	/* Dump local page 0 */
			dump_extend_phy_reg(arg, phy_base + offset, 16, 31, 1, 1);	/* Dump local page 1 */
			dump_extend_phy_reg(arg, phy_base + offset, 16, 31, 1, 2);	/* Dump local page 2 */
			dump_extend_phy_reg(arg, phy_base + offset, 16, 31, 1, 3);	/* Dump local page 3 */
		}
	} else {
		dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 0);	/* Dump local page 0 */
		dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 1);	/* Dump local page 1 */
		dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 2);	/* Dump local page 2 */
		dump_extend_phy_reg(arg, phy_addr, 16, 31, 1, 3);	/* Dump local page 3 */
	}
	return ret;
}